JP2613116B2 - Steering control device for self-propelled vehicles - Google Patents
Steering control device for self-propelled vehiclesInfo
- Publication number
- JP2613116B2 JP2613116B2 JP2093050A JP9305090A JP2613116B2 JP 2613116 B2 JP2613116 B2 JP 2613116B2 JP 2093050 A JP2093050 A JP 2093050A JP 9305090 A JP9305090 A JP 9305090A JP 2613116 B2 JP2613116 B2 JP 2613116B2
- Authority
- JP
- Japan
- Prior art keywords
- self
- azimuth
- propelled vehicle
- light
- angle
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Expired - Lifetime
Links
- 238000000034 method Methods 0.000 claims description 53
- 230000008569 process Effects 0.000 claims description 46
- 238000001514 detection method Methods 0.000 claims description 15
- 230000004044 response Effects 0.000 claims description 2
- 238000010586 diagram Methods 0.000 description 6
- 230000007704 transition Effects 0.000 description 3
- 230000000694 effects Effects 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 1
Landscapes
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Description
【発明の詳細な説明】 (産業上の利用分野) 本発明は、自走車の操向制御装置に関し、特に、工場
内の無人移動搬送装置、農業および土木機械等の自走車
の操向制御装置に関する。Description: TECHNICAL FIELD The present invention relates to a steering control device for a self-propelled vehicle, and more particularly, to a self-propelled vehicle such as an unmanned mobile transfer device in a factory, an agricultural and civil engineering machine, and the like. It relates to a control device.
(従来の技術) 従来、上記自走車のような移動体の現在位置を検知す
る装置として、移動体で発生した光ビームを、移動体を
中心として円周方向に走査する手段と、移動体とは離れ
た少なくとも3カ所に固定され、入射方向に光を反射す
る光反射手段と、該光反射手段からの反射光を受光する
受光手段とを具備した装置が提案されている(特開昭59
-67476号公報)。(Prior Art) Conventionally, as a device for detecting the current position of a moving body such as the above-mentioned self-propelled vehicle, means for scanning a light beam generated by the moving body in a circumferential direction around the moving body, There has been proposed an apparatus which is fixed to at least three places distant from the light reflecting means and includes light reflecting means for reflecting light in the incident direction, and light receiving means for receiving the reflected light from the light reflecting means (Japanese Patent Application Laid-Open No. HEI 9-28139). 59
-67476).
該装置は、移動体から見た前記3つの光反射手段相互
間の開き角を、前記受光手段と受光出力に基づいて検出
し、検出された開き角と、あらかじめ設定されている各
光反射手段の位置情報とに基づいて移動***置を演算す
るように構成されている。The apparatus detects an opening angle between the three light reflecting means as viewed from a moving body based on the light receiving means and the light receiving output, and detects the detected opening angle and each of predetermined light reflecting means. The position of the moving object is calculated based on the position information.
上記システムにおいては、自走車の傾斜は振動に起因
して光ビームを光反射手段に照射できなかったり、受光
手段で、前記光反射手段以外の物体からの反射光を受光
してしまう場合があった。予定の光反射手段からの反射
光が確実に受光されないと、自走車の位置が誤って算出
され、その結果、予定されたコースに沿って自走車を走
行させられなくなる。In the above system, the inclination of the self-propelled vehicle may not be able to irradiate the light beam to the light reflecting means due to vibration, or the light receiving means may receive reflected light from an object other than the light reflecting means. there were. If the reflected light from the planned light reflecting means is not reliably received, the position of the self-propelled vehicle is incorrectly calculated, and as a result, the self-propelled vehicle cannot be run along the planned course.
これに対し、光反射手段を4カ所に設置しておき、そ
のうちの1つを一時的に見失った場合には、他の3つの
光反射手段の位置に基づいて自己位置を算出し、見失っ
た光反射手段の方向を該算出結果から逆算することがで
きる(特願平1-1868号)。On the other hand, when the light reflecting means is installed in four places and one of them is temporarily lost, the self-position is calculated based on the positions of the other three light reflecting means, and the light reflecting means is lost. The direction of the light reflecting means can be calculated backward from the calculation result (Japanese Patent Application No. 1-1868).
また、光反射手段が3カ所にしか設定されていない場
合は、過去に検出された光反射手段の受光方向のデータ
に基づいて、次に該光反射手段が検出されるべき受光方
向を予測し、該予測方向において検出された受光信号
を、他の物体からの反射光と識別して予定の光反射手段
からの光であると判断することもできる。この方法によ
れば、予測方向において受光信号が検出されなかった場
合、該予測方向をそのまま予定の光反射手段からの反射
光として自走車の自己位置算出に用いることにより、一
時的な光反射手段の見失いによる問題点を解決できる。If the light reflecting means is set only at three positions, the light receiving direction of the light reflecting means to be detected next is predicted based on the data of the light receiving direction of the light reflecting means detected in the past. The light receiving signal detected in the predicted direction can be distinguished from the reflected light from another object, and can be determined as the light from the expected light reflecting means. According to this method, when a light receiving signal is not detected in the predicted direction, the predicted direction is used as it is as reflected light from the scheduled light reflecting means for the self-position calculation of the self-propelled vehicle, so that the temporary light reflection is achieved. The problem caused by the loss of the means can be solved.
(発明が解決しようとする課題) 上記の従来技術によれば、基準点に配置された光反射
手段を一時的に見失った場合にも予定された自走車が走
行コースを大きく逸脱することはない。しかしながら、
上記のように、逆算によって得られた光反射手段の位置
データや、推定した方位角データから得られた光反射手
段の位置データに基づいて算出された自走車の位置は必
ずしも正確とはいい難い。その結果、該自走車の位置デ
ータに基づいてフィードバック操向制御を行った場合、
自走車のふらつき走行(蛇行)の原因となる場合があ
る。(Problems to be Solved by the Invention) According to the above-described conventional technology, even when the light reflecting means arranged at the reference point is temporarily lost, the scheduled self-propelled vehicle does not greatly deviate from the traveling course. Absent. However,
As described above, the position of the self-propelled vehicle calculated based on the position data of the light reflecting means obtained by the back calculation and the position data of the light reflecting means obtained from the estimated azimuth angle data is not necessarily accurate. hard. As a result, when feedback steering control is performed based on the position data of the self-propelled vehicle,
This may cause the self-propelled vehicle to wander (meander).
一方、直進行程を走行した後、自走車の走行区域端部
の旋回行程を経て前記直進行程に隣接する次の直進行程
に移行するような走行形態においては、該直進行程から
旋回行程に移行するにあたり、旋回地点をちきんと守れ
ないと走行作業域から逸脱してしまうことになるため、
旋回指示を確実に行う必要がある。On the other hand, in a traveling mode in which the vehicle travels through the straight traveling process and then transitions to the next straight traveling process adjacent to the straight traveling process via the turning process at the end of the traveling area of the self-propelled vehicle, the transition from the straight traveling process to the turning process is performed. In doing so, if you do not keep the turning point properly, you will deviate from the traveling work area,
It is necessary to give a turning instruction reliably.
本発明の目的は、上記従来技術の問題点を解消し、光
反射手段を一時的に見失った場合でも自走車が蛇行をし
ないようにし、かつ、直進行程およびこれに連続する旋
回行程からなる走行コースの走行において自走車が直進
行程から旋回行程へ確実に移行できるようにするため自
走車の操向制御装置を提供することにある。SUMMARY OF THE INVENTION An object of the present invention is to solve the above-mentioned problems of the prior art, to prevent a self-propelled vehicle from meandering even when the light reflecting means is temporarily lost, and to comprise a straight traveling process and a continuous turning process. An object of the present invention is to provide a steering control device for a self-propelled vehicle so that the self-propelled vehicle can surely make a transition from a straight traveling process to a turning process when traveling on a traveling course.
(課題を解決するための手段および作用) 前記の問題点を解決し、目的を達成するために、本発
明は、自走車で発生した光ビームを該自走車を中心とし
て円周方向に走査する手段と、自走車から離れた基準点
に配置された光反射手段からの前記光ビームの反射光を
受光して自走車から見た前記光反射手段の方位角を測定
する手段と、該方位角に基づいて自走車を走行させる手
段と、前記測定された方位角に基づいて次回の走査で角
光反射手段が検出されるべき方位角を予測する手段と、
直進行程走行中に、前記予測された方位角で光反射手段
を検出できなかった場合に見失った光反射手段の方位角
を推定し、該推定方位角および他の検出された光反射手
段の方位角に基づいて自走車の位置を推定する手段と、
該自走車の位置情報に基づいて前進行程の自走車前進方
向の位置を照合することにより、前記直進行程からこれ
に続く旋回行程へ移行すべき位置に自走車が到達したか
否かを検出する手段と、該検出結果に基づいて、前記旋
回行程へ移行すべき位置へ到達していない場合には操舵
角を直進状態のままに固定し、前記旋回行程へ移行すべ
き位置へ到達した場合には自走車を旋回させる手段とを
具備した点に特徴がある。(Means and Actions for Solving the Problems) In order to solve the above problems and achieve the object, the present invention provides a light beam generated by a self-propelled vehicle in a circumferential direction around the self-propelled vehicle. Means for scanning, and means for receiving the reflected light of the light beam from the light reflecting means disposed at a reference point distant from the self-propelled vehicle and measuring the azimuth of the light reflecting means viewed from the self-propelled vehicle Means for driving the self-propelled vehicle based on the azimuth angle, means for predicting the azimuth angle to be detected by the angular light reflection means in the next scan based on the measured azimuth angle,
Estimating the azimuth of the light reflecting means that was lost when the light reflecting means could not be detected at the predicted azimuth during the straight traveling, and calculating the estimated azimuth and the azimuth of the other detected light reflecting means. Means for estimating the position of the vehicle based on the angle;
By comparing the position of the self-propelled vehicle in the forward direction of the self-propelled vehicle based on the position information of the self-propelled vehicle, it is determined whether or not the self-propelled vehicle has reached a position to shift from the straight traveling process to a subsequent turning process. Based on the detection result, the steering angle is fixed in a straight-ahead state when the vehicle has not reached the position to be shifted to the turning stroke, and reaches the position to be shifted to the turning stroke. In this case, there is provided a means for turning the self-propelled vehicle.
上記構成を有する本発明では、基準点を見失った時、
その地点が直進行程である場合は自走車の位置および進
行方向のすれを補正するように走行させる操舵角制御は
行わないようにする。但し、その場合でも見失った基準
点の推定方位角に基づいて自走車の位置検出は継続させ
る。そして、旋回行程開始地点に自走車が到達したこと
を該自走車の位置情報のうちの進行方向の位置情報に基
づいて判断し、自走車を旋回せることができる。In the present invention having the above configuration, when the reference point is lost,
If the point is about to travel straight ahead, steering angle control for traveling so as to correct the deviation of the position and traveling direction of the self-propelled vehicle is not performed. However, even in such a case, the position detection of the self-propelled vehicle is continued based on the estimated azimuth angle of the lost reference point. Then, it can be determined that the self-propelled vehicle has reached the turning stroke start point based on the position information in the traveling direction of the position information of the self-propelled vehicle, and the self-propelled vehicle can be turned.
(実施例) 以下に図面を参照して、本発明の一実施例を説明す
る。第9図は本発明の制御装置を搭載した自走車および
該自走車の走行領域に配設された光反射器の配置状態を
示す斜視図である。同図において、自走車1は例えば芝
刈り機等の農作業用自走車である。該自走車1の上部に
はモータ5によって駆動される回転テーブル4が設けら
れている。該回転テーブル4には光ビーム2Eを発生する
発光器2および該光ビームの反射光2Rを受ける受光器3
が搭載されている。Embodiment An embodiment of the present invention will be described below with reference to the drawings. FIG. 9 is a perspective view showing a self-propelled vehicle equipped with the control device of the present invention and a light reflector disposed in a traveling area of the self-propelled vehicle. In the figure, a self-propelled vehicle 1 is a self-propelled vehicle for agricultural work such as a lawnmower. A rotary table 4 driven by a motor 5 is provided above the self-propelled vehicle 1. The rotary table 4 has a light emitting device 2 for generating a light beam 2E and a light receiving device 3 for receiving the reflected light 2R of the light beam.
Is installed.
前記発光器2は光ビーム2Eを発生するための発光ダイ
オードを備え、受光器3は反射光2Rを受けてこれを電気
的信号に変換するフォトダイオードを備えている(共に
図示しない)。また、ロータリエンコーダ7は回転テー
ブル4の駆動軸と連動するように設けられいて、該ロー
タリエンコーダ7から出力されるパルスを計数すること
によって、回転テーブル4の回転角度が検出できる。The light emitting device 2 includes a light emitting diode for generating a light beam 2E, and the light receiving device 3 includes a photodiode for receiving the reflected light 2R and converting it into an electric signal (both are not shown). Further, the rotary encoder 7 is provided so as to be linked with the drive shaft of the rotary table 4, and the rotation angle of the rotary table 4 can be detected by counting the pulses output from the rotary encoder 7.
自走車1の作業区域の周囲には反射器6a〜6cが配設さ
れている。該反射器6a〜6cは入射した光を、その入射方
向に反射する反射面を具備しており、いわゆるコーナキ
ューブプリズム等周知の光反射手段が使用できる。Reflectors 6a to 6c are arranged around the work area of the vehicle 1. Each of the reflectors 6a to 6c has a reflecting surface for reflecting incident light in the incident direction, and a well-known light reflecting means such as a so-called corner cube prism can be used.
上記の構成により、発光器2で発生された光ビーム2E
は、例えば反時計方向に走査され、反射器6a〜6cでの反
射光2Rは反射器6a,6b,6cからの反射光の順番に受光器3
で検出され、該検出信号に基づいて反射器6a〜6cに対す
る自走車1の自己位置を検出して操向制御が行われる。With the above configuration, the light beam 2E generated by the light emitter 2
Is scanned in, for example, a counterclockwise direction, and the reflected light 2R from the reflectors 6a to 6c is reflected in the order of the reflected light from the reflectors 6a, 6b, and 6c.
The self-propelled vehicle 1 detects its own position with respect to the reflectors 6a to 6c based on the detection signal, and the steering control is performed.
ところで、自走車1の走行区域内あるいはその近辺に
前記反射器以外の反射物体または発光物体が存在してい
て、受光器3でこの反射物体からの光を検出することも
あり得るし、予定の反射器からの反射光を検出できない
こともあり得る。By the way, a reflecting object or a light emitting object other than the reflector exists in or near the traveling area of the self-propelled vehicle 1, and the light receiver 3 may detect light from the reflecting object, May not be able to detect the reflected light from the reflector.
したがって、本実施例では、検出された光が予定の反
射器からの光か否かを次のような処理によって識別する
ようにしている。Therefore, in the present embodiment, whether or not the detected light is light from the intended reflector is identified by the following processing.
第3図は基準点識別処理の説明図である。同図におい
て、作業区域22の周囲の基準点A〜Cには前記反射器6a
〜6cがそれぞ配置される。矢印29は自走車1から発射さ
れる光ビームの走査方向である。FIG. 3 is an explanatory diagram of the reference point identification processing. In the figure, reference points A to C around a work area 22 have the reflector 6a.
~ 6c are arranged respectively. The arrow 29 indicates the scanning direction of the light beam emitted from the vehicle 1.
図示のような配置において、自走車1では、前記受光
器3の検出信号に基づいて該自走車1から見た各基準点
の方位角が算出され、さらに、現時点までに検出された
方位角に基づいて次回の走査で検出されるべき基準点の
方位角が予測される。予測された方位角(予測方位角)
は角度θpa〜θpcで示す。各予測方位角θpa〜θpcから
光ビーム走査方向に角度θhだけ走査が進んだ方位に基
準点識別方位pa〜pcが設けられる。この基準点識別方位
pa〜pcに光ビームの走査が進む毎に、直前の基準点識別
方位から現基準点識別方位までに検出された光のうち、
予測方位角に最も近い方向からの入射光を、予定の基準
点に設置された反射器からの光であると判定する。In the arrangement shown in the figure, the azimuth of each reference point viewed from the self-propelled vehicle 1 is calculated based on the detection signal of the photodetector 3 in the self-propelled vehicle 1, and the azimuth detected up to the present time is further calculated. The azimuth of the reference point to be detected in the next scan is predicted based on the angle. Predicted azimuth (predicted azimuth)
Denotes the angles θpa to θpc. Reference point identification azimuths pa to pc are provided in azimuths obtained by scanning from the predicted azimuth angles θpa to θpc by the angle θh in the light beam scanning direction. This reference point identification direction
Each time the scanning of the light beam progresses from pa to pc, of the light detected from the immediately preceding reference point identification direction to the current reference point identification direction,
The incident light from the direction closest to the predicted azimuth is determined to be light from a reflector installed at a predetermined reference point.
例えば、基準点識別方位paにおいて、直前の基準点識
別方位pcから現在までに、ノイズ源N1,N2および基準点
Aに設置された反射器6aからの光を検出したとする。こ
の場合、これらの光の中から予測方位角θpaに最も近い
方向からの光、つまり基準点Aからの光を識別すること
ができる。For example, it is assumed that in the reference point identification direction pa, light from the noise sources N1, N2 and the reflector 6a installed at the reference point A is detected from the immediately preceding reference point identification direction pc to the present. In this case, light from the direction closest to the predicted azimuth angle θpa, that is, light from the reference point A, can be identified from these lights.
また、基準点の識別精度を向上させるために次の処理
を付加することもできる。すなわち、予測方位角の前後
に予定の範囲(前記角度θhと同等またはそれより小さ
い角度)を設け、予測方位角に最も近い方向からの光で
あっても該範囲からはずれていた場合には予定の基準点
を見失ったと判定する。予定の基準点を見失ったと判定
されると、走行コースの直進行程を走行中は、前記予測
方位角を用いて算出すると共に、該予測方位角または該
予測方位角に予定の角度を加算した角度を次回の予測方
位角として記憶する。Further, the following processing can be added to improve the reference point identification accuracy. That is, a predetermined range (an angle equal to or smaller than the angle θh) is provided before and after the predicted azimuth, and even if light from a direction closest to the predicted azimuth deviates from the range, a predetermined range is set. It is determined that the reference point has been lost. When it is determined that the planned reference point has been lost, while traveling the straight course of the traveling course, the angle is calculated using the predicted azimuth angle and the predicted azimuth angle or a predetermined angle added to the predicted azimuth angle. Is stored as the next predicted azimuth.
次に、自走車1の位置および進行方向を検出するため
の演算手順を説明する。第6図および第7図は、自走車
1の作業範囲を指示するための座標系における自走車1
および反射器6の位置を示す。Next, a calculation procedure for detecting the position and the traveling direction of the self-propelled vehicle 1 will be described. FIGS. 6 and 7 show the self-propelled vehicle 1 in a coordinate system for indicating the work range of the self-propelled vehicle 1.
And the position of the reflector 6.
第6図および第7図において、反射器6a〜6cがそれぞ
れ配置された基準点A,B,C、および自走車1の位置は、
基準点Bを原点とし、基準点BおよびCを結ぶ直線をx
軸とするx−y座標系で表される。In FIGS. 6 and 7, the reference points A, B, and C where the reflectors 6a to 6c are respectively arranged, and the position of the self-propelled vehicle 1 are as follows.
With reference point B as the origin, a straight line connecting reference points B and C is x
Expressed in an xy coordinate system as an axis.
同図から理解されるように、自走車1の位置Tは、三
角形ATBの外接円上に存在すると同時に、三角形BTCの外
接円上に存在する。したがって、自走車1の位置は三角
形ATBおよび三角形BTCのそれぞれの外接円QおよびPの
2つの交点を算出することによって求められる。As understood from the figure, the position T of the self-propelled vehicle 1 exists on the circumcircle of the triangle ATB and at the same time on the circumcircle of the triangle BTC. Accordingly, the position of the self-propelled vehicle 1 is obtained by calculating two intersections of the circumscribed circles Q and P of the triangle ATB and the triangle BTC, respectively.
図示のように、外接円QおよびPの一方の交点である
基準点Bを原点とし、外接円QおよびPの他方の交点T
を算出すれば自走車1の位置が確定できる。As shown, a reference point B, which is one of the intersections of the circumscribed circles Q and P, is set as the origin, and the other intersection T of the circumscribed circles Q and P is used as the origin.
Is calculated, the position of the self-propelled vehicle 1 can be determined.
自走車1の位置を確定する算出式は、特開平1-287415
号および特開平1-316808号に示されているので省略す
る。The formula for determining the position of the self-propelled vehicle 1 is described in Japanese Patent Application Laid-Open No. 1-287415.
The description is omitted here because it is disclosed in Japanese Patent Application Laid-Open No. Hei.
自走車1の進行方向は次の式を用いて算出される。第
7図において、自走車1の進行方向とx軸とのなす角度
をθfとし、該進行方向を基準とした基準点A,B,Cの方
位角をθa,θb,θcとした場合、 θf= 360°−tan-1{y/(xc−x)}−θc ……(1) となる。The traveling direction of the self-propelled vehicle 1 is calculated using the following equation. In FIG. 7, when the angle between the traveling direction of the self-propelled vehicle 1 and the x axis is θf, and the azimuths of the reference points A, B, and C with respect to the traveling direction are θa, θb, θc, θf = 360 ° −tan −1 {y / (xc−x)} − θc (1)
該自走車1の位置および進行方向は、上述の算出式お
よび上記算出式(1)を用い、後述の位置・進行方向演
算部13において算出される。The position and the traveling direction of the self-propelled vehicle 1 are calculated by a position / traveling direction calculation unit 13 described later using the above-described calculation formula and the calculation formula (1).
次に、上記手順によって算出された自走車1の位置情
報に基づく自走車1の操向制御について説明する。第8
図は自走車1の走行コースと基準点との位置関係を示す
図であり、基準点Bを原点とし、基準点BおよびCを通
る直線をx軸とする座標系で、自走車1の位置および該
自走車1による作業区域22が示されている。Next, the steering control of the self-propelled vehicle 1 based on the position information of the self-propelled vehicle 1 calculated by the above procedure will be described. 8th
The figure shows the positional relationship between the traveling course of the self-propelled vehicle 1 and the reference point. The coordinate system uses the reference point B as the origin and the straight line passing through the reference points B and C as the x-axis. And a work area 22 for the self-propelled vehicle 1 are shown.
点R(Xret,Yret)は自走車1の戻り位置を示し、座
標(Xst,Yst)、(Xst,Ye)、(Xe,Yst)、(Xe,Ye)で
示される点で結ばれた領域が作業区域22である。自走車
1の位置Tは座標(Xp,Yp)で示す。The point R (Xret, Yret) indicates the return position of the self-propelled vehicle 1 and is connected by points indicated by coordinates (Xst, Yst), (Xst, Ye), (Xe, Yst), (Xe, Ye). The area is the work area 22. The position T of the self-propelled vehicle 1 is indicated by coordinates (Xp, Yp).
自走車1は作業開始位置Sから走行を開始し、直進行
程および1つの直進行程から次の直進行程へ移行するた
めの旋回行程を順に走行して芝刈り等、予定の作業を行
う。The self-propelled vehicle 1 starts traveling from the work start position S, and performs a scheduled operation such as lawn mowing by sequentially traveling a straight traveling process and a turning process for shifting from one straight traveling process to the next straight traveling process.
行き方向(図面上で上方向)での旋回開始点および戻
り方向(図面上で下方向)での旋回開始点はy座標Yeお
よびYstで示した。すなわち、自走車1の直進中、該自
走車1のy座標がYeまたはYstになった時点で旋回を開
始する。The turning start point in the approach direction (upward on the drawing) and the turning start point in the return direction (downward in the drawing) are indicated by y-coordinates Ye and Yst. That is, while the self-propelled vehicle 1 is traveling straight, the turn starts when the y coordinate of the self-propelled vehicle 1 becomes Ye or Yst.
旋回終了点のy座標も旋回開始点のy座標と同一であ
るが、旋回中は自走車1の座標の算出が正確に行えない
ため、旋回終了点に自走車1が到達したか否かの判断
は、自走車1のy座標によって行わず、自走車1から見
た基準点の方位角データによって行う。Although the y-coordinate of the turning end point is the same as the y-coordinate of the turning start point, it is not possible to accurately calculate the coordinates of the self-propelled vehicle 1 during the turning, so whether or not the self-propelled vehicle 1 has reached the turning end point. This determination is not made based on the y-coordinate of the self-propelled vehicle 1 but based on the azimuth data of the reference point viewed from the self-propelled vehicle 1.
旋回行程での操向制御および旋回終了判断の制御の例
は特開平1-316808号および特願平2-19293号に記載され
ている。Examples of steering control and turning end determination control during a turning stroke are described in JP-A-1-316808 and Japanese Patent Application No. 2-19293.
なお、第8図においては、説明を簡単にするため、作
業区域22の4辺をx軸またはy軸に平行にした例を示し
たが、作業区域22の周囲に基準点A,B,Cを配置してあれ
ば、作業区域22の形状および作業区域22の4辺の向きは
任意である。FIG. 8 shows an example in which the four sides of the work area 22 are parallel to the x-axis or the y-axis for the sake of simplicity, but reference points A, B, and C are provided around the work area 22. Are arranged, the shape of the work area 22 and the directions of the four sides of the work area 22 are arbitrary.
次に、第1図に示したブロック図を参照し、上記の操
向制御を行うための制御装置の機能構成を説明する。同
図において、鎖線で囲まれた部分はマイクロコンピュー
タで構成できる。Next, a functional configuration of a control device for performing the above-described steering control will be described with reference to the block diagram shown in FIG. In the figure, a portion surrounded by a chain line can be constituted by a microcomputer.
第1図において、発光器2から射出された光ビーム2E
は、前記回転テーブル4の回動方向に走査され、反射器
6(6a〜6c)によって反射される。該反射器6a〜6cの反
射光2Rは受光器3で受光される。In FIG. 1, the light beam 2E emitted from the light emitting device 2 is shown.
Is scanned in the direction of rotation of the turntable 4 and is reflected by the reflectors 6 (6a to 6c). The reflected light 2R from the reflectors 6a to 6c is received by the light receiver 3.
カウンタ9では、前記回転テーブル4の回転に伴って
ロータリエンコーダ7から出力されるパルスが計数され
る。該パルスの計数値は受光器3において光を検出する
毎に方位角検出部11に転送される。方位角検出部11で
は、供給されたパルス数に基づいて反射器6a〜6cの方位
角が算出される。The counter 9 counts pulses output from the rotary encoder 7 as the rotary table 4 rotates. The count value of the pulse is transferred to the azimuth detecting unit 11 each time light is detected in the light receiver 3. The azimuth detecting unit 11 calculates the azimuth of the reflectors 6a to 6c based on the number of supplied pulses.
方位角検出部11で検出された方位角は方位角記憶部12
に転送されて記憶され、該方位角記憶部12にそれまでに
蓄積されたデータは、識別タイミング発生部23から供給
される識別タイミング信号に応答して方位角識別部24に
転送される。前記識別タイミング信号は、方位角予測演
算部27で算出された予測方位角で示される方位を予定角
度θhだけ通過した方位まで走査が進んだ時点、すなわ
ち、前記基準点識別方位pa〜pcに光ビームの走査が進ん
だ時点で出力される。このために、識別タイミング発生
部23ではロータリエンコーダ7の出力パルスを、方位角
予測演算部27で算出された予測方位角に相当する予定数
だけ取込んだ時点で識別タイミング信号を出力する。The azimuth detected by the azimuth detection unit 11 is stored in the azimuth storage unit 12.
The data which has been transferred and stored in the azimuth storage unit 12 so far is transferred to the azimuth identification unit 24 in response to the identification timing signal supplied from the identification timing generation unit 23. The identification timing signal is transmitted to the azimuth indicated by the predicted azimuth calculated by the azimuth prediction operation unit 27 up to the azimuth that has passed by the predetermined angle θh, that is, the light is transmitted to the reference point identification azimuths pa to pc. It is output at the time when beam scanning has progressed. For this purpose, the identification timing generator 23 outputs an identification timing signal when the output pulses of the rotary encoder 7 are fetched by a predetermined number corresponding to the predicted azimuth calculated by the azimuth prediction calculator 27.
方位角識別部24は、供給された方位角の中から方位角
予測演算部27で算出される予測方位角に最も近い方向で
検出された光を予定の基準点に配置された反射器からの
反射光であると判断する。この判断によって決定された
反射器の方位角データは、次回の走査で検出されるべき
反射器の方位角を方位角予測演算部27において予測する
際に利用される。すなわち、方位角識別部24で決定され
た方位角の、実験的に得られる予定の関数によって予測
方位角は求められる。予測方位角は予定の関数に基づい
て手法に限らず、方位角識別部24で得られた今回および
前回の方位角の差を今回の方位角に加算して求めるよう
にしてもよい。The azimuth discriminating unit 24 detects light detected in the direction closest to the predicted azimuth calculated by the azimuth prediction calculating unit 27 from the supplied azimuths from a reflector arranged at a predetermined reference point. Judge as reflected light. The azimuth data of the reflector determined by this determination is used when the azimuth prediction calculation unit 27 predicts the azimuth of the reflector to be detected in the next scan. That is, the predicted azimuth angle is obtained by an experimentally obtained function of the azimuth angle determined by the azimuth identification unit 24. The predicted azimuth is not limited to the method based on the scheduled function, and may be obtained by adding the difference between the current azimuth and the previous azimuth obtained by the azimuth identification unit 24 to the current azimuth.
なお、方位角識別部24で予定の基準点が検出されなか
った場合は見失い信号aが出力される。If the azimuth discriminating unit 24 does not detect a predetermined reference point, a missing signal a is output.
方位角識別部24で検出された方位角は開き角演算部10
に入力され、自走車1から見た反射器6a〜6c相互間の開
き角が演算される。The azimuth detected by the azimuth discriminating unit 24 is the open angle calculating unit 10
And the opening angle between the reflectors 6a to 6c viewed from the self-propelled vehicle 1 is calculated.
位置・進行方向演算部13では、開き角に基づいて自走
車1の現在の位置座標が演算されるとともに、方位角に
基づいて自走車1の進行方向が演算される。この演算結
果は比較部25に入力される。比較部25では、走行コース
設定部16に設定されている走行コースを表すデータと、
前記位置・進行方向演算部13で得られた自走車1の位置
(Xp,Yp)および信号方向θfとが比較される。The position / traveling direction calculation unit 13 calculates the current position coordinates of the vehicle 1 based on the opening angle, and calculates the traveling direction of the vehicle 1 based on the azimuth. This calculation result is input to the comparison unit 25. In the comparison unit 25, data representing the traveling course set in the traveling course setting unit 16;
The position (Xp, Yp) of the self-propelled vehicle 1 obtained by the position / traveling direction calculation unit 13 and the signal direction θf are compared.
この比較結果、つまり予定の走行コースとのx方向偏
位置Δxおよびずれ角度Δθは切換部33を介して操舵部
14に入力される。The comparison result, that is, the deviation position Δx and the deviation angle Δθ in the x direction from the planned traveling course are determined by the steering unit via the switching unit 33.
Entered in 14.
該操舵部14に比較部25からの信号の他、切換部33の切
換え位置に応じて直進用角度設定部30および旋回用角度
設定部31から操舵角固定用角度データが供給される。直
進用角度設定部30には、操舵角を直進用に固定するた
め、ずれ角度Δθとして“0°”を設定する。また、旋
回用角度設定部31には、操舵角を旋回用に固定するた
め、ずれ角度Δθとして大きなずれ角度を設定してお
く。そうすることにより、操舵部14では該大きなずれ角
度を補正しようとして、最大限に大きな操舵角が選択さ
れる。In addition to the signal from the comparison unit 25, the steering unit 14 is supplied with steering angle fixing angle data from the straight traveling angle setting unit 30 and the turning angle setting unit 31 according to the switching position of the switching unit 33. In order to fix the steering angle for straight traveling, “0 °” is set as the deviation angle Δθ in the straight traveling angle setting unit 30. Further, in the turning angle setting unit 31, a large deviation angle is set as the deviation angle Δθ in order to fix the steering angle for turning. By doing so, the steering unit 14 selects the largest possible steering angle in an attempt to correct the large deviation angle.
前記比較部25では、自走車1が旋回すべき位置に存在
しているか否かを自走車1のy座標Ypによって判断し、
自走車1が旋回すべき位置にある場合は旋回行程検出信
号bを高レベル“H"にし、直進行程に存在する場合は旋
回部検出信号bを低レベル“L"にする。見失い信号aと
旋回行程検出信号bとは切換信号発生器32に供給され
る。該切換信号発生部32では次のような論理演算によっ
て切換信号cを出力する。The comparing unit 25 determines whether or not the self-propelled vehicle 1 is present at a position where the self-propelled vehicle 1 should turn, based on the y coordinate Yp of the self-propelled vehicle 1,
When the self-propelled vehicle 1 is at a position to be turned, the turning stroke detection signal b is set to a high level “H”, and when the self-propelled vehicle 1 is in a straight traveling range, the turning portion detection signal b is set to a low level “L”. The missing signal a and the turning stroke detection signal b are supplied to the switching signal generator 32. The switching signal generator 32 outputs the switching signal c by the following logical operation.
前記旋回行程検出信号bが“L"で、方位角識別部24か
らの見失い信号aが“H"の時、つまり直進行程走行中に
基準点を見失った場合、切換部33を直進用角度設定部30
と操舵部14とを接続するような切換信号cが出力され
る。また、旋回行程検出信号bが“H"の場合、つまり自
走車1が旋回すべき位置に存在している間は、切換部33
を、旋回用角度設定部31と操舵部14とを接続するような
切換信号cが出力される。さらに、見失い信号aが“L"
で旋回部検出信号bが“L"の場合、基準点を見失わずに
直進している場合は、比較部25と操舵部14とを接続する
ような切換信号を出力する。When the turning stroke detection signal b is "L" and the missing signal a from the azimuth discriminating unit 24 is "H", that is, when the reference point is lost while traveling straight ahead, the switching unit 33 sets the straight traveling angle. Part 30
A switching signal c for connecting the steering wheel 14 to the steering unit 14 is output. When the turning stroke detection signal b is “H”, that is, while the self-propelled vehicle 1 is at a position to turn, the switching unit 33
A switching signal c for connecting the turning angle setting unit 31 and the steering unit 14 is output. Further, the missing signal a is "L"
If the turning section detection signal b is "L" and the vehicle is traveling straight without losing reference to the reference point, a switching signal for connecting the comparing section 25 and the steering section 14 is output.
操舵部14は前記比較部25から供給される偏位置Δxお
よびずれ角度Δθ、ならびに直進用角度設定部30および
旋回用角度設定部31から供給される角度データに基づい
て自走車の前輪17に連結された操舵モータ28を駆動す
る。操舵モータ28による前輪17の操舵角は、自走車1の
前輪に設けられた舵角センサ15で検出され、操舵部14に
フィードバックされる。駆動制御部18はエンジン19の始
動・停止、および該エンジン19の動力を後輪21に伝達す
るクラッチ20の動作を制御する。The steering unit 14 controls the front wheel 17 of the self-propelled vehicle based on the offset position Δx and the deviation angle Δθ supplied from the comparison unit 25, and the angle data supplied from the straight traveling angle setting unit 30 and the turning angle setting unit 31. The connected steering motor 28 is driven. The steering angle of the front wheels 17 by the steering motor 28 is detected by a steering angle sensor 15 provided on the front wheels of the self-propelled vehicle 1 and fed back to the steering unit 14. The drive control unit 18 controls start / stop of the engine 19 and operation of the clutch 20 that transmits the power of the engine 19 to the rear wheels 21.
さらに、基準点の識別精度を向上させるためには次の
機能が付加することができる。すなわち、範囲設定部26
に設定された角度範囲を示す値を方位角識別部24に供給
し、前記予測方位角に最も近い方向で検出された光が該
予定の角度範囲で検出されたものか否かを識別する機能
が付加される。Further, the following function can be added to improve the identification accuracy of the reference point. That is, the range setting unit 26
A function of supplying a value indicating the angle range set to the azimuth angle identification unit 24 to identify whether or not the light detected in the direction closest to the predicted azimuth is detected in the predetermined angle range. Is added.
該識別機能による識別の結果、予測方位角に最も近い
方向で検出された光が予定の範囲内にある場合は該方位
角を使って開き角を算出し、予定の範囲か外れている場
合は、方位角予測演算部27で算出された予測方位角を使
って開き角を算出するようにする。As a result of the identification by the identification function, if the light detected in the direction closest to the predicted azimuth angle is within a predetermined range, the opening angle is calculated using the azimuth angle, and if it is out of the predetermined range, The opening angle is calculated using the predicted azimuth calculated by the azimuth prediction calculating unit 27.
この範囲設定部26に設定された範囲内からの光を、予
定の基準点からの反射光を判断し、該光の方位角を使っ
て開き角を演算するか、入射光の方位角が該範囲設定部
26に設定された範囲内にあるか否かの判断を経ずに決定
した方位角を使って開く角を演算するかは、該自走車1
による作業形態とか種類に応じて必要とされる精度の程
度に応じて任意に選択すればよい。Light from within the range set in the range setting unit 26 is determined as reflected light from a predetermined reference point, and the azimuth angle of the light is used to calculate the opening angle, or the azimuth angle of the incident light is Range setting section
Whether the opening angle is calculated using the azimuth determined without going through the determination as to whether the vehicle is within the range set in 26 or not is determined by the vehicle 1
May be arbitrarily selected according to the degree of accuracy required in accordance with the type of work or the type of operation.
第2図のフローチャートに従って制御手順を説明す
る。The control procedure will be described with reference to the flowchart of FIG.
まず、ステップS1では、自走車1と点Rから作業開始
位置まで、無線操縦により移動させる。ステップS2で
は、走行コースのx座標XnとしてXstをセットし、走行
コースを決定する。First, in step S1, the self-propelled vehicle 1 is moved from the point R to the work start position by radio control. In step S2, Xst is set as the x-coordinate Xn of the traveling course, and the traveling course is determined.
ステップS3では、自走車1の走行を開始させる。 In step S3, the traveling of the self-propelled vehicle 1 is started.
ステップS4では、受光器3で基準点または他の光源か
らの光を受光したか否かの判断がなされる。光が検出さ
れるとステップS5に進んで後述の受光処理が行われ、光
が検出されない場合はステップS6に進む。In step S4, it is determined whether or not the light receiver 3 has received light from a reference point or another light source. When the light is detected, the process proceeds to step S5, and the light receiving process described later is performed. When the light is not detected, the process proceeds to step S6.
ステップS6では、受光した反射光のうちどれが予定通
りの基準点からの光かを決定するための、基準点識別処
理を行うタイミングに至ったか否かを判断する。該判断
は、方位角予測演算部27で演算された予測方位角θpa〜
θpcから予定の角度θhだけ進んだ基準点識別方位pa〜
pcまで走査が進んだか否かによって行われる。In step S6, it is determined whether or not it is time to perform the reference point identification process for determining which of the received reflected lights is the light from the expected reference point. The determination is based on the predicted azimuth θpa calculated by the azimuth prediction
Reference point discriminating direction pa ~ advanced from θpc by a predetermined angle θh
This is performed depending on whether the scan has progressed to pc.
ステップS6の判断が肯定となるまでステップ4〜S6は
繰返され、該判断が肯定となるとステップS7に進み、基
準点識別処理が実行される。Steps S4 to S6 are repeated until the determination in step S6 becomes affirmative. If the determination is affirmative, the process proceeds to step S7, where a reference point identification process is executed.
ステップS8では、基準点識別処理によって予定の基準
点、つまり自走車の位置計算にその方位角を用いる基準
点が検出されたか見失ったかの判断がなされる。該判断
は後述する基準点処理においてセットされる基準点見失
いフラグに従う。In step S8, a determination is made as to whether a predetermined reference point, that is, a reference point using the azimuth angle for calculating the position of the self-propelled vehicle, has been detected or lost by the reference point identification processing. This determination is based on a reference point missing flag set in a reference point process described later.
基準点が検出されたならばステップS9に進み、検出さ
れた基準点の方位角に基づいて自走車1の位置T(Xp,Y
p)および進行方向θfの演算が行われる。If the reference point has been detected, the process proceeds to step S9, and based on the azimuth of the detected reference point, the position T (Xp, Y
The calculation of p) and the traveling direction θf are performed.
ステップS10では、走行コースからのずれ量(ΔX=X
p−Nn、Δθf)が演算され、ステップS11では、算出さ
れたずれ量に応じ、前記操舵部14において操舵角制御が
行われる。In step S10, the deviation amount from the traveling course (ΔX = X
p−Nn, Δθf) are calculated, and in step S11, the steering angle control is performed in the steering unit 14 according to the calculated shift amount.
ステップS12では、自走車1がy軸方向において、原
点から遠ざかる方向(行き方向)に走行しているか、原
点に近づく方向(戻り方向)に走行しているかが判断さ
れる。In step S12, it is determined whether the self-propelled vehicle 1 is traveling in a direction away from the origin (forward direction) or traveling in a direction approaching the origin (return direction) in the y-axis direction.
行き方向であれば、ステップS13において、一行程が
終了したか(Yp>Ye)否かが判断され、戻り方向であれ
ば、ステップS14において、一行程が終了(Yp<Yst)し
たか否かが判断される。ステップS13またはS14におい
て、一行程が終了していないと判断されればステップS4
に戻る。If it is the direction of the way, it is determined in step S13 whether one stroke has been completed (Yp> Ye), and if it is the return direction, in step S14, whether or not one stroke has been completed (Yp <Yst). Is determined. If it is determined in step S13 or S14 that one stroke is not completed, step S4
Return to
ステップS13またはS14において、一行程が終了したと
判断されれば、次はステップS15において全行程が終了
した(Xn>Xe−L)か否かの判断が行われる。If it is determined in step S13 or S14 that one stroke has been completed, then it is determined in step S15 whether all the strokes have been completed (Xn> Xe-L).
全行程が終了していなければ、ステップS15からステ
ップS16に移って自走車1のUターン制御が行われる。
Uターン制御は、前記位置・進行方向演算部13で演算さ
れた自走車1の位置情報を操舵部14にフィードバックす
るステップS9〜S11の処理によって行われる直進行程の
操向制御とは別の方式で行われる。If the entire process has not been completed, the process moves from step S15 to step S16, and the U-turn control of the self-propelled vehicle 1 is performed.
The U-turn control is different from the steering control in the straight traveling range performed by the processing of steps S9 to S11 in which the position information of the self-propelled vehicle 1 calculated by the position / traveling direction calculation unit 13 is fed back to the steering unit 14. It is done in a manner.
すなわち、旋回行程では前記旋回用角度設定部31の設
定角度に基づいて操舵部14で決定された角度に操舵角を
固定して走行させる。そして、自走車1に対する各基準
点A,B,Cの方位角の少なくとも1つが予定の角度に合致
するか予定の角度範囲内に入った時点で旋回のための操
舵各の固定を解除する。That is, in the turning stroke, the vehicle travels with the steering angle fixed to the angle determined by the steering unit 14 based on the angle set by the turning angle setting unit 31. Then, when at least one of the azimuths of the reference points A, B, and C with respect to the self-propelled vehicle 1 matches a predetermined angle or falls within a predetermined angle range, the steering lock for turning is released. .
ステップS17では、XnにXn+Lがセットされ、次の一
行程の走行コースが設定される。走行コースが設定され
ればステップS4に戻って、前記処理が繰返される。In step S17, Xn is set to Xn + L, and the traveling course of the next one stroke is set. If the traveling course is set, the process returns to step S4, and the above processing is repeated.
全行程が終了したならば戻り位置R(Xret,Yret)へ
戻って(ステップS18)、走行が停止される(ステップS
19)。When all the steps are completed, the process returns to the return position R (Xret, Yret) (step S18), and the traveling is stopped (step S18).
19).
一方、前記ステップS8の判断において基準点を見失っ
たと判断された場合は、ステップS20に移行して自走車
1のy座標のみの演算を行う。該y座標は、ステップS1
3およびS14において、自走車1が旋回位置に到達したか
否かを判断する場合のデータとして使用される。On the other hand, if it is determined in step S8 that the reference point has been lost, the process proceeds to step S20, and only the y coordinate of the self-propelled vehicle 1 is calculated. The y coordinate is calculated in step S1.
3 and S14 are used as data for determining whether or not the self-propelled vehicle 1 has reached the turning position.
ステップS21では、前記直進用角度設定部30の設定角
度に基づいて操舵部14で決定された角度に操舵角を固定
する。すなわち、自走車1を直進状態に固定する。In step S21, the steering angle is fixed to the angle determined by the steering unit 14 based on the angle set by the straight angle setting unit 30. That is, the self-propelled vehicle 1 is fixed in the straight traveling state.
次に、前記ステップS5およびS7の受光処理および基準
点識別処理について説明する。受光処理のフローチャー
トを第5図に示す。Next, the light receiving process and the reference point identifying process in steps S5 and S7 will be described. FIG. 5 shows a flowchart of the light receiving process.
同図において、ステップS50では、光を検出したこと
を記憶するため、受光フラグに“1"をセットする。In the figure, in step S50, "1" is set to a light receiving flag to store that light has been detected.
ステップS51では、検出した光の発生源の方位角を方
位角記憶部12に記憶する。In step S51, the azimuth of the detected light source is stored in the azimuth storage unit 12.
基準点識別処理のフローチャートを第4図に示す。該
フローチャートでは、予測方位角に最も近い検出方位角
を、前記範囲設定部26に設定された予定の角度でさらに
絞り込んで基準点を識別する手順の例を示す。FIG. 4 shows a flowchart of the reference point identification processing. The flowchart shows an example of a procedure for identifying a reference point by further narrowing down the detected azimuth angle closest to the predicted azimuth angle by a predetermined angle set in the range setting unit 26.
同図において、ステップS70では、識別すべき基準点
を区別するためのポールカウンタの値(以下、単にポー
ルカウンタという)nをインクリメントする。該ポール
カウンタは各基準点に対応させてある。すなわち、ポー
ルカウンタ“1"は基準点Aに、ポールカウンタ“2"は基
準点Bに、ポールカウンタ“3"は基準点Cにそれぞれ対
応している。In the figure, in step S70, a value (hereinafter simply referred to as a pole counter) n of a pole counter for distinguishing a reference point to be identified is incremented. The pole counter is associated with each reference point. That is, the pole counter “1” corresponds to the reference point A, the pole counter “2” corresponds to the reference point B, and the pole counter “3” corresponds to the reference point C.
ポールカウンタの初期値が“0"であれば、ステップS7
0の処理によってボールカウンタは“1"になり、これに
対応する基準点はAということになる。本実施例では初
期値を“0"とする。If the initial value of the pole counter is "0", step S7
By the processing of 0, the ball counter becomes "1", and the reference point corresponding to this becomes A. In this embodiment, the initial value is “0”.
ステップS71では、受光フラグの判別を行い、受光フ
ラグが“1"にならばステップS72に進み、受光フラグが
“0"ならばステップS80にジャンプする。In step S71, the light reception flag is determined. If the light reception flag is "1", the flow proceeds to step S72. If the light reception flag is "0", the flow jumps to step S80.
ステップS72では、前記方位角記憶部12に記憶された
光の発生源の方位角の中で、予測方位角θpn(ポールカ
ウンタは“1"になっているので予測方位角θpa)に最も
近いものを、予定された基準点の方位角であると仮定
し、その値を角度θsとして記憶する。In step S72, among the azimuths of the light sources stored in the azimuth storage unit 12, the azimuth closest to the predicted azimuth θpn (the predicted azimuth θpa since the pole counter is “1”) Is assumed to be the azimuth of the planned reference point, and the value is stored as the angle θs.
ステップS73では、受光数が“2"以上か、つまり方位
角記憶部12に複数の方位角が記憶されているか否かを判
断することによってノイズの有無を判断する。In step S73, the presence or absence of noise is determined by determining whether the number of received light is “2” or more, that is, whether a plurality of azimuths are stored in the azimuth storage unit 12.
該ステップS73の判断が肯定ならば、1つ以上のノイ
ズを検出したとしてステップS74に移行し、ノイズ処理
としてノイズを検出したことを記憶する。この記憶デー
タによって後で作業環境の状況を知る手掛かりが得ら
れ、ノイズ源の除去などの対策を講じることが容易にな
る。If the determination in step S73 is affirmative, it is determined that one or more noises have been detected, and the process proceeds to step S74 to store that noise has been detected as noise processing. The stored data provides a clue to know the situation of the work environment later, and makes it easy to take measures such as removal of noise sources.
ステップS73に判断が否定ならば、ステップS75に進ん
で前記仮に決定された方位角θsと予測方位角θpn(予
測方位角θpa)との差が前記角度θhより小さいか否か
の判断を行う。該差が角度θhより大きい場合は、仮に
決定した方位角θsは予定された基準点の方位角ではな
く、ノイズ源の方位角であったと判断してステップS79
に進み、前記ステップS74と同様のノイズ処理を行う。If a negative determination is made in step S73, the flow advances to step S75 to determine whether the difference between the tentatively determined azimuth θs and the predicted azimuth θpn (predicted azimuth θpa) is smaller than the angle θh. If the difference is larger than the angle θh, it is determined that the temporarily determined azimuth θs is not the azimuth of the predetermined reference point but the azimuth of the noise source, and step S79 is performed.
And the same noise processing as in step S74 is performed.
該ノイズ処理の後は、ステップS80に進み、基準点見
失い処理として予測方位角θpn(予測方位角θpa)を予
定された基準点の方位角θn(θa)としてセットす
る。After the noise processing, the flow advances to step S80 to set the predicted azimuth θpn (predicted azimuth θpa) as the azimuth θn (θa) of the scheduled reference point as the reference point missing processing.
ステップS81では、基準点を見失ったことを示す見失
いnフラグ(ポールカウンタは“1"になっているので見
失いAフラグ)をセットする。In step S81, a missing n flag indicating that the reference point has been missed (A missing flag because the pole counter is "1") is set.
一方、前記差が角度θhより小さい場合は、仮定した
方位角θsは予定された基準点の方位角を示すものとし
て決定し、ステップS76に進む。On the other hand, if the difference is smaller than the angle θh, the assumed azimuth θs is determined as indicating the azimuth of the predetermined reference point, and the process proceeds to step S76.
ステップS76では、前回の処理で決定された方位角θ
nと今回の処理で決定された方位角θsとに基づいて、
次回の処理時に同一の基準点が検出されるべき予測方位
角を、算出式{θs+(θs−θn)}を用いて算出す
る。In step S76, the azimuth θ determined in the previous process
n and the azimuth θs determined in this process,
A predicted azimuth angle at which the same reference point should be detected in the next process is calculated using a calculation formula {θs + (θs−θn)}.
ステップS77では、方位角θnを角度θsで更新す
る。In step S77, the azimuth θn is updated with the angle θs.
ステップS78では、基準点nを見失ったことを示す見
失いnフラグをクリアする。In step S78, a missing n flag indicating that the reference point n has been lost is cleared.
ステップS82では、次の、基準点識別方位Pn+1(す
なわち、基準点識別方位pb)として前回の走査時に基準
点Bを検出した時に算出した予測方位角θpn+1に予定
角度θhを加算した角度をセットする。In step S82, an angle obtained by adding the scheduled angle θh to the predicted azimuth angle θpn + 1 calculated when the reference point B was detected in the previous scan is set as the next reference point identification azimuth Pn + 1 (that is, the reference point identification azimuth pb). .
ステップS83では、受光フラグをクリアする。 In step S83, the light receiving flag is cleared.
ステップS84では、方位角記憶部12の記憶データを消
去する。In step S84, the data stored in the azimuth storage unit 12 is deleted.
ステップS85では、ポールカウンタが“3"か否かを判
別する。該値“3"は設置されている基準点の数であり、
基準点の設置数nに応じて該値を設定しておく。In the step S85, it is determined whether or not the pole counter is "3". The value “3” is the number of set reference points,
The value is set according to the number n of the reference points.
設置されている基準点の数とポールカウンタとが一致
した場合、すなわちすべての基準点に対して識別処理が
終了したと判定された場合は、ステップS86でポールカ
ウンタに“0"をセットして第2図に示した処理に戻る。If the number of installed reference points matches the pole counter, that is, if it is determined that the identification processing has been completed for all the reference points, “0” is set in the pole counter in step S86. Returning to the processing shown in FIG.
ポールカウンタが“1"の時、次回の処理では、ステッ
プ70によってポールカウンタはインクリメントされて
“2"になり、該ポールカウンタ“2"に対応する基準点B
の識別処理が行われる。When the pole counter is "1", in the next process, the pole counter is incremented to "2" by step 70, and the reference point B corresponding to the pole counter "2" is set.
Is performed.
以下、同様にして基準点Cの識別処理も行われる。 Hereinafter, the identification process of the reference point C is performed in the same manner.
以上の説明のように、本実施例では、受光器3によっ
て複数の光が検出されたような場合に、該複数の光のう
ち予測方位角に最も近い方向からの光であって、しかも
前記予測方位角から予定角度以上外れていない方向から
の光を、予定された基準点からの反射光であると判定す
るようにした。As described above, in the present embodiment, when a plurality of lights are detected by the light receiver 3, the plurality of lights are lights from a direction closest to the predicted azimuth angle, and Light from a direction that does not deviate from the predicted azimuth by more than a predetermined angle is determined to be reflected light from a predetermined reference point.
そして、直進行程の走行中に予定の基準点(位置演算
をするのに用いる基準点)を見失った場合は、操舵角を
直進状態に固定して走行させるようにした。When a predetermined reference point (a reference point used for position calculation) is lost during traveling in the straight traveling range, the vehicle is driven with the steering angle fixed at the straight traveling state.
また、予定の基準点を見失った場合は、前記予測方位
角を見失った基準点の方位角であると推定し、該方位角
を使用して自走車1の位置の算出は継続させるようにし
た。したがって、予定の基準点を見失った場合でも、旋
回行程へ移行すべき位置をほぼ正確に検知できる。Further, when the reference point is lost, the estimated azimuth is estimated to be the azimuth of the reference point that has lost the azimuth, and the calculation of the position of the self-propelled vehicle 1 is continued using the azimuth. did. Therefore, even if the intended reference point is lost, the position to shift to the turning stroke can be detected almost exactly.
(発明の効果) 以上の説明から明らかなように、本発明によれば、次
のような効果を得ることができる。(Effects of the Invention) As is clear from the above description, according to the present invention, the following effects can be obtained.
(1) 直進中に一時的に基準点を見失っても、操舵角
を直進状態に固定して走行させるので、直進行程の走行
における蛇行の程度を低減できる。(1) Even if the reference point is temporarily lost while traveling straight, the vehicle travels with the steering angle fixed in the straight traveling state, so that the degree of meandering in traveling in the straight traveling can be reduced.
(2) 基準点を一時的に見失っても、ほぼ予定された
位置で旋回行程に移行することができる。またこれによ
って操舵角を予定角度に固定して旋回した場合に、旋回
終了後、次の直進行程へ円滑に移行できる。(2) Even if the reference point is temporarily lost, it is possible to shift to a turning stroke at a substantially planned position. Further, when the vehicle turns with the steering angle fixed at the predetermined angle, the vehicle can smoothly shift to the next straight travel after the turn.
(3) 基準点を見失った場合、推定された方位角デー
タによる自走車の位置データは自走車の直進走行の制御
には使用しない。しかし、旋回位置に到達したか否か
は、この推定された方位角データによる自走車の位置デ
ータによって継続して判断することができるので、直進
行程から旋回行程へ確実に移行することができる。(3) When the reference point is lost, the position data of the self-propelled vehicle based on the estimated azimuth data is not used for controlling the straight running of the self-propelled vehicle. However, whether or not the vehicle has reached the turning position can be continuously determined based on the position data of the self-propelled vehicle based on the estimated azimuth angle data. Therefore, it is possible to reliably shift from the straight traveling process to the turning process. .
第1図は本発明の一実施例の機能を示すブロック図、第
2図は操向制御のフローチャート、第3図は基準点識別
処理の説明図、第4図は基準点識別処理のフローチャー
ト、第5図は受光処理のフローチャート、第6図は自走
車の位置検出の原理説明図、第7図は自走車の進行方向
検出の原理説明図、第8図は自走車の走行コースと反射
器の配置状態を示す図、第9図は自走車の走行コースと
反射器の配置状態を示す斜視図である。 1……自走車、2……発光器、3……受光器、6,6a〜6c
……反射器、11……方位角検出部、12……方位角記憶
部、13……位置・進行方向演算部、23……識別タイミン
グ発生部、24……方位角識別部、30……直進用角度設定
部、31……旋回用角度設定部FIG. 1 is a block diagram showing functions of one embodiment of the present invention, FIG. 2 is a flowchart of steering control, FIG. 3 is an explanatory diagram of a reference point identification process, FIG. 4 is a flowchart of a reference point identification process, FIG. 5 is a flowchart of the light receiving process, FIG. 6 is a diagram illustrating the principle of position detection of the self-propelled vehicle, FIG. 7 is a diagram illustrating the principle of detection of the traveling direction of the self-propelled vehicle, and FIG. FIG. 9 is a perspective view showing the traveling course of the self-propelled vehicle and the arrangement of the reflectors. 1 ... self-propelled vehicle, 2 ... light-emitting device, 3 ... light-receiving device, 6, 6a-6c
… Reflector, 11 azimuth detection unit, 12 azimuth storage unit, 13 position / travel direction calculation unit, 23 identification timing generation unit, 24 azimuth identification unit, 30 Angle setting section for straight ahead, 31 ... Angle setting section for turning
Claims (3)
中心として円周方向に走査し、自走車から離れた複数の
基準点に配置された光反射手段からの光ビームの反射光
を受光して自走車から見た該光反射手段の方位角を測定
し、その結果に基づいて直進行程およびこれに連続する
旋回行程からなる走行コースに自走車を走行させるため
の自走車の操向制御装置において、 前記測定された方位角に基づいて次回の走査で各光反射
手段が検出されるべき方位角を予測する方位角予測手段
と、 前記予測された方位角からの入射光が検出されなかった
場合に、予定の光反射手段を見失ったとして見失い信号
を出力する基準点識別手段と、 前記見失い信号に応答し、見失った光反射手段の方位角
を推定する手段と、 該推定された方位角および他の検出された光反射手段の
方位角に基づいて自走車の位置を推定する手段と、 前記推定された自走車の位置情報に基づいて自走車前進
方向の位置を照合することにより、旋回行程へ移行すべ
き位置に自走車が到達したか否かを検出する手段と、 この検出結果に基づいて、前記旋回行程へ移行すべき位
置へ到達していない場合には操舵角を直進状態に固定し
て走行させ、前記旋回行程へ移行すべき位置へ到達した
場合には自走車を旋回させる手段とを具備したことを特
徴とする自走車の操向制御装置。1. A light beam generated by a self-propelled vehicle is scanned in a circumferential direction around the self-propelled vehicle, and a light beam from light reflecting means arranged at a plurality of reference points distant from the self-propelled vehicle. To measure the azimuthal angle of the light reflecting means as viewed from the self-propelled vehicle by receiving the reflected light from the self-propelled vehicle, and based on the result, drive the self-propelled vehicle on a running course consisting of a straight traveling process and a continuous turning process. An azimuth prediction unit for predicting an azimuth at which each light reflection unit should be detected in the next scan based on the measured azimuth, and the predicted azimuth. A reference point identifying means for outputting a missing signal when the predetermined light reflecting means is missed when no incident light from the light is detected, and estimating an azimuth angle of the missing light reflecting means in response to the missing light signal. Means for detecting said estimated azimuth and other Means for estimating the position of the self-propelled vehicle based on the azimuth angle of the light reflecting means, and collating the position of the self-propelled vehicle in the forward direction based on the estimated position information of the self-propelled vehicle. Means for detecting whether or not the self-propelled vehicle has reached the position to shift to, and based on the detection result, if the vehicle has not reached the position to shift to the turning stroke, the steering angle is set to the straight traveling state. Means for rotating the self-propelled vehicle when the vehicle travels in a fixed manner and reaches a position to be shifted to the turning stroke.
れた方位角であると推定することを特徴とする請求項1
記載の自走車の操向制御装置。2. The method according to claim 1, wherein the azimuth of the lost light reflecting means is estimated to be the predicted azimuth.
A steering control device for a self-propelled vehicle according to the above.
光反射手段の方位角から予定角度だけ光ビームの走査が
進んだ方向毎に基準点識別方位を設定し、直前の基準点
識別方位から現基準点識別方位までの間に検出した入射
光のうち、予測された方位角に最も近い角度からの入射
光を予定の基準点に配置された光反射手段からの反射光
であると判断する手段であることを特徴とする請求項1
または2記載の自走車の操向制御装置。3. The reference point identification means sets a reference point identification azimuth for each direction in which the scanning of the light beam has advanced by a predetermined angle from the predicted azimuth angle of each of the light reflecting means. Of the incident light detected from the azimuth to the current reference point identification azimuth, the incident light from the angle closest to the predicted azimuth is the reflected light from the light reflecting means arranged at the predetermined reference point. 2. A means for judging.
Or the steering control device for a self-propelled vehicle according to 2.
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2093050A JP2613116B2 (en) | 1990-04-10 | 1990-04-10 | Steering control device for self-propelled vehicles |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2093050A JP2613116B2 (en) | 1990-04-10 | 1990-04-10 | Steering control device for self-propelled vehicles |
Publications (2)
Publication Number | Publication Date |
---|---|
JPH03291702A JPH03291702A (en) | 1991-12-20 |
JP2613116B2 true JP2613116B2 (en) | 1997-05-21 |
Family
ID=14071683
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
JP2093050A Expired - Lifetime JP2613116B2 (en) | 1990-04-10 | 1990-04-10 | Steering control device for self-propelled vehicles |
Country Status (1)
Country | Link |
---|---|
JP (1) | JP2613116B2 (en) |
Family Cites Families (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JPS595B2 (en) * | 1979-04-23 | 1984-01-05 | オムロン株式会社 | Automatic travel control device for tillers |
JP2717800B2 (en) * | 1988-06-17 | 1998-02-25 | 本田技研工業株式会社 | Steering control device for self-propelled vehicles |
-
1990
- 1990-04-10 JP JP2093050A patent/JP2613116B2/en not_active Expired - Lifetime
Also Published As
Publication number | Publication date |
---|---|
JPH03291702A (en) | 1991-12-20 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
US5187662A (en) | Steering control system for moving vehicle | |
JP2717800B2 (en) | Steering control device for self-propelled vehicles | |
JP2802502B2 (en) | Self-propelled vehicle steering position detecting device and reference point detecting device therefor | |
US5068795A (en) | System for detecting position of moving vehicle | |
JP2769837B2 (en) | Self-propelled vehicle position detection device | |
JP2868847B2 (en) | Steering control device for self-propelled vehicles | |
JP2688960B2 (en) | Self-propelled vehicle steering position detection device | |
JP2613116B2 (en) | Steering control device for self-propelled vehicles | |
US5019990A (en) | Position detector for moving vehicle | |
JP2513514B2 (en) | Steering control device for self-propelled vehicle | |
JPH0833767B2 (en) | Steering control device for self-propelled vehicle | |
JP2538344B2 (en) | Steering control device for self-propelled vehicle | |
JP2802506B2 (en) | Self-propelled vehicle position detection device | |
JP2717826B2 (en) | Steering control device for self-propelled vehicles | |
JPH0833766B2 (en) | Self-propelled vehicle steering position detection device | |
JP2535213B2 (en) | Self-propelled vehicle steering position detection device | |
JPH03244006A (en) | Steering controller for self-traveling vehicle | |
JP2802525B2 (en) | Traveling course setting device for self-propelled vehicles | |
JP3338565B2 (en) | Traveling area setting method and traveling control device for moving object | |
JPH0833768B2 (en) | Self-propelled vehicle steering position detection device | |
JPH03154904A (en) | Position controller for self-traveling vehicle | |
JPH03237505A (en) | Steering controller for free-running vehicle | |
JPH047716A (en) | Steering control device for autonomous traveling vehicle | |
JP2754267B2 (en) | Position control device for self-propelled vehicles | |
JP2802521B2 (en) | Reference point detection device for position control of self-propelled vehicles |