JPH04361189A - Moving body position detector - Google Patents
Moving body position detectorInfo
- Publication number
- JPH04361189A JPH04361189A JP16233891A JP16233891A JPH04361189A JP H04361189 A JPH04361189 A JP H04361189A JP 16233891 A JP16233891 A JP 16233891A JP 16233891 A JP16233891 A JP 16233891A JP H04361189 A JPH04361189 A JP H04361189A
- Authority
- JP
- Japan
- Prior art keywords
- light
- swing
- light beam
- azimuth
- reference point
- 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.)
- Pending
Links
- 238000001514 detection method Methods 0.000 claims abstract description 106
- 230000010355 oscillation Effects 0.000 claims description 17
- 230000007423 decrease Effects 0.000 claims description 8
- 230000003247 decreasing effect Effects 0.000 claims description 2
- 238000000034 method Methods 0.000 description 162
- 230000008569 process Effects 0.000 description 158
- 238000012545 processing Methods 0.000 description 57
- 238000004364 calculation method Methods 0.000 description 29
- 238000010586 diagram Methods 0.000 description 24
- 230000003287 optical effect Effects 0.000 description 19
- 230000006870 function Effects 0.000 description 16
- 238000005259 measurement Methods 0.000 description 9
- 230000007246 mechanism Effects 0.000 description 8
- 230000004044 response Effects 0.000 description 6
- 230000008859 change Effects 0.000 description 3
- 244000145845 chattering Species 0.000 description 3
- XEEYBQQBJWHFJM-UHFFFAOYSA-N Iron Chemical compound [Fe] XEEYBQQBJWHFJM-UHFFFAOYSA-N 0.000 description 2
- 230000005856 abnormality Effects 0.000 description 2
- 238000013500 data storage Methods 0.000 description 2
- 230000003111 delayed effect Effects 0.000 description 2
- NJPPVKZQTLUDBO-UHFFFAOYSA-N novaluron Chemical compound C1=C(Cl)C(OC(F)(F)C(OC(F)(F)F)F)=CC=C1NC(=O)NC(=O)C1=C(F)C=CC=C1F NJPPVKZQTLUDBO-UHFFFAOYSA-N 0.000 description 2
- 238000012935 Averaging Methods 0.000 description 1
- 230000001174 ascending effect Effects 0.000 description 1
- 238000006243 chemical reaction Methods 0.000 description 1
- 238000013480 data collection Methods 0.000 description 1
- 239000000284 extract Substances 0.000 description 1
- 230000012447 hatching Effects 0.000 description 1
- 229910052742 iron Inorganic materials 0.000 description 1
- 230000001678 irradiating effect Effects 0.000 description 1
- 239000000696 magnetic material Substances 0.000 description 1
- 230000002250 progressing effect Effects 0.000 description 1
- 230000009467 reduction Effects 0.000 description 1
- 230000001360 synchronised effect Effects 0.000 description 1
Landscapes
- Aerials With Secondary Devices (AREA)
- Optical Radar Systems And Details Thereof (AREA)
Abstract
Description
【0001】0001
【産業上の利用分野】本発明は、移動体の位置検出装置
に関し、特に、農業および土木作業に使用される自走式
機械や、工場内で使用される自動搬送装置などの移動体
の位置検出装置に関する。[Field of Industrial Application] The present invention relates to a device for detecting the position of a moving object, and in particular, the present invention relates to a device for detecting the position of a moving object, and particularly to a device for detecting the position of a moving object such as a self-propelled machine used in agriculture and civil engineering work, or an automatic conveyance device used in a factory. This invention relates to a detection device.
【0002】0002
【従来の技術】従来、上記のような移動体の現在位置を
検知する装置として、移動体で発生された光ビームを移
動体を中心として円周方向に走査する手段と、移動体か
ら離れた少なくとも3か所に固定され、入射方向に光を
反射する再帰反射手段(以下、光反射手段という)と、
この光反射手段によって反射された光を受光する受光手
段とを具備した装置が提案されている(特開昭59−6
7476号公報)。2. Description of the Related Art Conventionally, devices for detecting the current position of a moving object as described above include means for scanning a light beam generated by the moving object in a circumferential direction centered on the moving object, and a means for scanning a light beam generated by the moving object in the circumferential direction, Retroreflection means (hereinafter referred to as light reflection means) fixed at at least three locations and reflecting light in the incident direction;
A device equipped with a light receiving means for receiving the light reflected by the light reflecting means has been proposed (Japanese Patent Laid-Open No. 59-6
Publication No. 7476).
【0003】この装置では、前記移動体から見た前記3
つの光反射手段相互間の開き角を前記受光手段の出力信
号に基づいて検出する。そして、検出された開き角とあ
らかじめ設定されている各光反射手段の位置を表す情報
(位置情報)とから移動体の位置を演算するように構成
している。[0003] In this device, the three
The opening angle between the two light reflecting means is detected based on the output signal of the light receiving means. The position of the moving object is calculated from the detected opening angle and preset information (position information) representing the position of each light reflecting means.
【0004】上記の装置においては、移動体つまり自走
車が傾斜した状態のまま走行したり、走行中に揺れたり
することによって、自走車から発せられた光ビームを光
反射手段に照射できない場合がある。そうすると、光反
射手段で反射されるべき前記光ビームの反射光が受光手
段で検出されない。これとは反対に、予定の光反射手段
以外の反射物体からの余計な光を受光してしまうような
場合もあった。光反射手段からの光を検出できなかった
り、誤って他からの光を予定の光反射手段の反射光とし
て検出してしまうと、自走車の位置を正確に算出できず
、予定されたコース上に自走車を走らせられなくなる場
合がある。[0004] In the above device, the light beam emitted from the self-propelled vehicle cannot be irradiated onto the light reflecting means because the moving object, that is, the self-propelled vehicle, runs in an inclined state or shakes while traveling. There are cases. Then, the reflected light of the light beam that should be reflected by the light reflecting means is not detected by the light receiving means. On the contrary, there have been cases where extra light is received from a reflecting object other than the intended light reflecting means. If the light from the light reflecting means cannot be detected, or if light from another source is mistakenly detected as reflected light from the intended light reflecting means, the position of the self-propelled vehicle cannot be accurately calculated, and the planned course cannot be accurately calculated. It may become impossible to drive a self-propelled vehicle over the top.
【0005】これに対し、本出願人は、次のような制御
装置を提案した(特開平2−109107号公報)。こ
の装置では、今回および前回の走査によってすでに検出
された自走車の進行方向を基準とした各光反射手段の方
位角データに基づき、その次の走査で同じ光反射手段が
検出されるはずの方位角を予測する。そして、この方位
角で示される予測方位から入射する光を予定の反射手段
からの正常な反射光であると判断する。そして、もしも
予測方位から光が入射されないことが繰返された場合に
は自走車を停止させるようにしている。[0005] In response, the present applicant proposed the following control device (Japanese Patent Laid-Open No. 109107/1999). In this device, based on the azimuth data of each light reflecting means with respect to the traveling direction of the self-propelled vehicle already detected in the current and previous scans, it is assumed that the same light reflecting means will be detected in the next scan. Predict azimuth. Then, the light incident from the predicted direction indicated by this azimuth angle is determined to be normal reflected light from the intended reflecting means. If light is not incident from the predicted direction repeatedly, the self-propelled vehicle is stopped.
【0006】また、本出願人は、前記予測方位で光が検
出されない場合の応急的な処置として、光反射手段が少
なくとも前記予測方位の近辺に存在するはずであるとい
う判断のもとに、この予測方位データを実際の方位角の
代わりに使用して自走車の位置検出を行うようにした制
御装置も提案した(特願平2−12424号)。[0006] Furthermore, as an emergency measure when light is not detected in the predicted direction, the applicant has determined that a light reflecting means should exist at least in the vicinity of the predicted direction. A control device was also proposed that uses predicted azimuth data in place of the actual azimuth to detect the position of a self-propelled vehicle (Japanese Patent Application No. 12424/1999).
【0007】この制御装置では、光反射手段の見失いが
一時的な場合は、自走車を停止させないで、予測方位を
実際の光反射手段が存在する方位であるとみなし、この
予測方位データを使用して自走車の位置を検知する。こ
のような処理を行っても、光反射手段の見失いが一時的
であれば、予測方位と実際の方位との誤差は小さいので
実用上の支障はない。しかし、自走車が走行する路面の
状態によっては、光反射手段を頻繁に見失ったり見失い
が長時間にわたることがあり、このような場合には、ま
た新たな対応手段が必要になる。[0007] In this control device, if the light reflection means is temporarily lost, the predicted direction is assumed to be the direction where the light reflection means actually exists, without stopping the self-propelled vehicle, and this predicted direction data is used. Used to detect the location of a self-propelled vehicle. Even if such processing is performed, if the loss of sight of the light reflecting means is temporary, there is no practical problem because the error between the predicted orientation and the actual orientation is small. However, depending on the condition of the road surface on which the self-propelled vehicle travels, the light reflecting means may be frequently lost or lost sight of for a long period of time, and in such cases, a new countermeasure is required.
【0008】これに対して、上記のような、光反射手段
を見失った後の対応ではなく、光反射手段を見失う前の
対応策も考えられている。例えば、光ビームを光反射手
段に確実に照射できるようにすることを目的として、発
生した光ビームをガルバノミラーやポリゴンミラーを利
用して上下方向に高速に振動させつつ水平方向に走査さ
せるようなビーム光走査装置が提案されている(特開昭
60−242313号公報)。このように光ビームを上
下方向にも走査することによって、自走車が路面の傾斜
地部分を走行したり揺れたりしても、自走車から投射す
る光ビームが光反射手段に照射できる確率が高くなる。[0008]In response to this problem, countermeasures are being considered not only after the light reflecting means is lost as described above, but also before the light reflecting means is lost. For example, in order to reliably irradiate the light beam onto the light reflecting means, the generated light beam may be vibrated in the vertical direction at high speed using a galvanometer mirror or a polygon mirror and scanned in the horizontal direction. A beam light scanning device has been proposed (Japanese Unexamined Patent Publication No. 60-242313). By scanning the light beam in the vertical direction in this way, the probability that the light beam projected from the self-propelled vehicle will irradiate the light reflecting means is increased even when the self-propelled vehicle travels on a sloped part of the road surface or shakes. It gets expensive.
【0009】図32は、このような従来の装置による光
ビームの走査軌跡(光跡)を示す図である。同図(a)
は光ビームを水平方向に回転走査させつつ、この光ビー
ムをガルバノミラーで上下方向に高速で揺動走査した場
合の光跡の一部分を示す。同図(b)は前記光ビームの
上下方向の高速揺動走査をポリゴンミラーで行った場合
の光跡の一部分を示す。FIG. 32 is a diagram showing a scanning locus (light trail) of a light beam by such a conventional device. Figure (a)
shows a portion of the light trail when the light beam is rotated and scanned in the horizontal direction and the light beam is oscillated and scanned in the vertical direction at high speed using a galvanometer mirror. FIG. 2B shows a portion of the light trail when the light beam is scanned by a polygon mirror in a high-speed swinging motion in the vertical direction.
【0010】0010
【発明が解決しようとする課題】上記のビーム光走査装
置では、次のような問題点があった。ガルバノミラーを
利用して所定の振れ幅で光ビームを上下方向にも揺動走
査する上記の方式では、自走車と光反射手段との距離が
大きくなるに従って光反射手段位置での光跡の振れ幅が
大きくなるとともに波長も長くなる。そのために、例え
ば図32(a)に示すように光反射手段6が光ビームと
交差できなくなってしまうことが有り得る。SUMMARY OF THE INVENTION The above beam light scanning device has the following problems. In the above method in which a galvanometer mirror is used to swing and scan the light beam in the vertical direction with a predetermined swing width, the light trace at the light reflecting means position increases as the distance between the self-propelled vehicle and the light reflecting means increases. As the amplitude of vibration increases, the wavelength also increases. Therefore, for example, as shown in FIG. 32(a), the light reflecting means 6 may not be able to intersect with the light beam.
【0011】また、ポリゴンミラーを利用した方式にお
いても、自走車と光反射手段6との距離が大きくなるに
従って光反射手段位置での光跡の間隔が広くなる。その
ために、例えば図32(b)に示すように光反射手段6
と光ビームとが交差できなくなってしまうことが有り得
る。[0011] Also, in the system using a polygon mirror, as the distance between the self-propelled vehicle and the light reflecting means 6 increases, the distance between the light traces at the position of the light reflecting means becomes wider. For this purpose, for example, as shown in FIG. 32(b), the light reflecting means 6
It is possible that the light beam and the light beam cannot intersect with each other.
【0012】光跡の波長もしくは間隔を小さくして光反
射手段6に光ビームの光跡を交差させやすくするために
は、回転走査に対する上下揺動走査の速度比を上げる必
要がある。つまり、上下揺動走査の駆動速度を大きくす
るか回転走査速度を小さくすることが必要になる。とこ
ろが、ガルバノミラーやポリゴンミラーの駆動速度を大
きくするのは機械構造的な制約もあって大変難しい。In order to make it easier for the light beam to cross the light reflecting means 6 by reducing the wavelength or interval of the light trace, it is necessary to increase the speed ratio of the vertically oscillating scan to the rotational scan. In other words, it is necessary to increase the drive speed for vertical swing scanning or to decrease the rotational scanning speed. However, increasing the driving speed of galvano mirrors and polygon mirrors is extremely difficult due to mechanical structural constraints.
【0013】また、水平方向の走査速度を小さくすると
、一定時間あたりの受光データ数が少なくなって位置検
出精度が低下し、特に、自走車等のような移動体の位置
検出用として使用する場合、検出精度の低下が著しい。[0013] Furthermore, if the horizontal scanning speed is reduced, the number of received light data per certain period of time decreases, and the position detection accuracy decreases. In this case, the detection accuracy decreases significantly.
【0014】本発明は、上記従来技術の問題点に対して
、移動体から投射する光ビームを各光反射手段に高い確
率で照射できるように構成することによって、位置検出
精度を高めることができる移動体の位置検出装置を提供
することにある。[0014] In order to solve the problems of the prior art described above, the present invention can improve position detection accuracy by configuring a light beam projected from a moving object to be irradiated onto each light reflecting means with high probability. An object of the present invention is to provide a position detection device for a moving body.
【0015】[0015]
【課題を解決するための手段】前記の問題点に対処し、
位置検出精度を高めるという目的を達成するために、本
発明は、移動体に搭載された光ビーム発生手段から投射
された光ビームを上下方向に揺動させる揺動走査手段と
、上下方向の揺動1サイクル中に前記移動体を中心とし
て光ビームを複数回円周方向に回転走査する回転走査手
段と、前記移動体から離れた位置に設置された光反射手
段で反射された前記光ビームの反射光を検出するため前
記移動体に搭載された受光手段と、この受光手段で検出
された反射光の方位角を記憶する手段と、前記反射光が
検出されていることを条件に前記揺動走査幅を小さく設
定する揺動幅設定手段と、前記揺動幅に応じ、この揺動
幅が小さく設定されたときは高い揺動走査速度を設定し
、大きく設定されたときは低い揺動走査速度を設定する
揺動走査速度設定手段とを具備し、前記記憶された方位
角に基づいて前記揺動走査に同期して前記移動体の位置
を算出するように構成した点に特徴がある。[Means for solving the problem] Addressing the above problems,
In order to achieve the objective of increasing position detection accuracy, the present invention provides a swing scanning means for vertically swinging a light beam projected from a light beam generating means mounted on a moving body; a rotating scanning means for rotationally scanning a light beam in the circumferential direction centering on the moving body multiple times during one cycle of movement; and a light beam reflected by a light reflecting means installed at a position away from the moving body. a light receiving means mounted on the movable body for detecting the reflected light; a means for storing the azimuth of the reflected light detected by the light receiving means; and a means for storing the azimuth of the reflected light detected by the light receiving means; an oscillation width setting means for setting a scan width small; and a oscillation width setting means for setting a high oscillation scanning speed when the oscillation width is set small, and a low oscillation scan speed when the oscillation width is set large, according to the oscillation width; The present invention is characterized in that it comprises a swing scan speed setting means for setting a speed, and is configured to calculate the position of the moving body in synchronization with the swing scan based on the stored azimuth.
【0016】[0016]
【作用】上記の特徴を有する本発明では、前記光ビーム
の揺動の1サイクル中に複数回の回転走査が行われる。
したがって、移動体を中心とする円筒面を想定した場合
、その円筒面上で光ビームによる網目状の光跡が描かれ
る。[Operation] In the present invention having the above characteristics, rotational scanning is performed a plurality of times during one cycle of the swinging of the light beam. Therefore, assuming a cylindrical surface centered on the moving body, a mesh-like light trail is drawn by the light beam on the cylindrical surface.
【0017】すなわち、垂直方向に立てられた光反射手
段の近辺において、揺動の1サイクル中、垂直方向の高
さが異なる数回以上の回転走査が行われる。その結果、
揺動1サイクル中に高い確率で光跡が光反射手段を横切
ることになり、それだけ光反射手段で反射された光ビー
ムを受光する確率が高くなる。That is, in the vicinity of the vertically erected light reflecting means, rotational scanning is performed several times or more at different heights in the vertical direction during one swing cycle. the result,
The light trace crosses the light reflecting means with a high probability during one cycle of swinging, and the probability of receiving the light beam reflected by the light reflecting means increases accordingly.
【0018】また、上下方向揺動走査幅は、反射光が検
出されている条件の下に、小さく設定されていて、それ
に合わせて走査速度を速くできるので、揺動走査に同期
させて位置算出した場合、位置算出間隔が短縮される。
したがって、この位置算出の結果に基づいて行われる各
種制御、例えば移動体の操舵制御において、操舵間隔が
短縮され、移動体の誘導精度を向上させられる。In addition, the vertical swing scanning width is set small under the condition that reflected light is detected, and the scanning speed can be increased accordingly, so position calculation can be performed in synchronization with the swing scanning. In this case, the position calculation interval is shortened. Therefore, in various controls performed based on the result of this position calculation, such as steering control of the moving body, the steering interval is shortened, and the accuracy of guiding the moving body is improved.
【0019】[0019]
【実施例】以下に、図面を参照して本発明の一実施例を
説明する。図2は本発明の位置検出装置を搭載し、所定
の領域を走行する自走車を示す斜視図である。図2にお
いて、移動体としての自走車1が走行している領域の周
囲には、入射した光をその入射方向に反射する反射面を
有する光反射器(以下、単に反射器という)6a〜6d
が配設されている。反射器6a〜6dの反射面には、コ
ーナキューブプリズム等周知の光反射手段が使用されて
いる。自走車1は、例えばその下面に図示しない芝刈作
業用カッタブレードを有する芝刈り機である。自走車1
の上部には、光ビーム走査装置(以下、単に走査装置と
いう)2が搭載されている。この走査装置2は、光ビー
ム2Eを発生する発光器、および前記反射器6a〜6d
で反射された光ビーム2Eの反射光2Rを受ける受光器
を有する。発光器および受光器は後で詳しく説明する内
側リング部材14の下面に固定されたケーシング3に収
容されている。前記発光器は発光ダイオードを有し、受
光器は入射された光を電気的信号に変換するフォトダイ
オードを有している。DESCRIPTION OF THE PREFERRED EMBODIMENTS An embodiment of the present invention will be described below with reference to the drawings. FIG. 2 is a perspective view showing a self-propelled vehicle equipped with the position detection device of the present invention and traveling in a predetermined area. In FIG. 2, light reflectors (hereinafter simply referred to as reflectors) 6a to 6a having reflective surfaces that reflect incident light in the direction of incidence are located around the area where the self-propelled vehicle 1 as a moving body is traveling. 6d
is installed. Known light reflecting means such as a corner cube prism is used for the reflecting surfaces of the reflectors 6a to 6d. The self-propelled vehicle 1 is, for example, a lawn mower having a cutter blade (not shown) for mowing work on its lower surface. Self-propelled car 1
A light beam scanning device (hereinafter simply referred to as a scanning device) 2 is mounted on the top of the scanner. This scanning device 2 includes a light emitter that generates a light beam 2E, and the reflectors 6a to 6d.
It has a light receiver that receives the reflected light 2R of the light beam 2E reflected by the light beam 2E. The light emitter and the light receiver are housed in a casing 3 fixed to the lower surface of an inner ring member 14, which will be described in detail later. The light emitter has a light emitting diode, and the light receiver has a photodiode that converts incident light into an electrical signal.
【0020】発光器から出た光ビームは回転ミラー(以
下、単にミラーという)4で直角方向に屈折反射される
ことにより、方向転換されて走査装置2から外部に投射
される。ミラー4はモータ5によって回転中心軸8のま
わりで矢印17の方向に回転され、このミラー4の回転
によって光ビーム2Eは回転中心軸8を中心として矢印
R方向つまり円周方向に回転走査される。ミラー4の回
転位置で決まる光ビーム2Eの投射方向つまりモータ5
の回転角度はエンコーダ7で検出される。The light beam emitted from the light emitter is refracted and reflected in a right angle direction by a rotating mirror (hereinafter simply referred to as a mirror) 4, thereby changing its direction and being projected from the scanning device 2 to the outside. The mirror 4 is rotated by the motor 5 around the central axis of rotation 8 in the direction of the arrow 17, and by the rotation of the mirror 4, the light beam 2E is rotated and scanned in the direction of the arrow R, that is, in the circumferential direction, about the central axis of rotation 8. . The projection direction of the light beam 2E is determined by the rotational position of the mirror 4, that is, the motor 5
The rotation angle of is detected by the encoder 7.
【0021】さらに、前記走査装置2は、光ビーム2E
の光跡で描かれる回転走査面の角度を連続変化(揺動走
査)させるためのジンバル揺動機構を有している。この
揺動機構は、ブラケット9の軸12およびブラケット1
0の図示しない軸に対して揺動自在に軸支された外側リ
ング部材11と、この外側リング部材11の内側に設け
られた内側リング部材14とを有する。この内側リング
部材14は、前記外側リング部材11の支軸の延長線と
直交する線上で外側リング部材11に設けられた軸13
およびこの軸13と対向する位置に設けられた他方の軸
20(図1に示す)によって揺動自在に軸支されている
。Furthermore, the scanning device 2 includes a light beam 2E.
It has a gimbal swing mechanism to continuously change the angle of the rotating scanning surface drawn by the light trail (swing scanning). This swing mechanism consists of a shaft 12 of the bracket 9 and a shaft 12 of the bracket 1.
It has an outer ring member 11 that is pivotably supported on a shaft (not shown) at 0, and an inner ring member 14 that is provided inside the outer ring member 11. This inner ring member 14 is connected to a shaft 13 provided on the outer ring member 11 on a line perpendicular to the extension line of the support shaft of the outer ring member 11.
It is pivotably supported by the other shaft 20 (shown in FIG. 1) provided at a position facing this shaft 13.
【0022】ジンバル揺動機構は揺動駆動用のステップ
モータ(以下、単にモータという)15aおよび15b
によって駆動される。すなわち、前記内側リング部材1
4はモータ15aで駆動され、外側リング部材11はモ
ータ15bでそれぞれ駆動される。The gimbal swing mechanism includes swing drive step motors (hereinafter simply referred to as motors) 15a and 15b.
driven by. That is, the inner ring member 1
4 is driven by a motor 15a, and the outer ring member 11 is driven by a motor 15b.
【0023】このジンバル揺動機構によって、ミラー4
の回転中心軸8は垂直から角度φだけ傾斜するように取
付けられており、かつその傾斜方向(以下、揺動方向と
いう)は連続的に変化して、矢印17aの方向に回動す
る。なお、前記回転中心軸8の傾斜角度φは、前記モー
タ15a,15bの回転角で決定される。このように回
転中心軸8が傾斜角度φを有することによって光ビーム
2Eの回転走査による走査面の角度は連続的に変化する
。すなわち、光ビーム2Eの投射方向が上下方向に連続
的に変化し、揺動走査されるのである。With this gimbal swing mechanism, the mirror 4
The rotation center shaft 8 of is mounted so as to be inclined by an angle φ from the vertical, and the direction of inclination (hereinafter referred to as the swinging direction) changes continuously and rotates in the direction of the arrow 17a. Incidentally, the inclination angle φ of the rotation center axis 8 is determined by the rotation angle of the motors 15a and 15b. Since the central axis of rotation 8 has the inclination angle φ in this manner, the angle of the scanning surface due to rotational scanning of the light beam 2E changes continuously. That is, the projection direction of the light beam 2E changes continuously in the vertical direction, and swing scanning is performed.
【0024】次に、前記走査装置およびジンバル揺動機
構の揺動駆動装置について図1を参照して詳述する。図
1は自走車1に搭載された走査装置2の要部断面図であ
り、図2と同符号は同一または同等部分を示す。まず、
走査装置2について説明する。ミラー4は台座4aを介
してモータ5の軸の一端5aに取付けられていている。
一方、モータ5の軸の他端5bは連結金具19によって
エンコーダ7の軸7aと連結されている。エンコーダ7
の出力パルスは図示しない制御装置に送信され、ミラー
4の回転角度や回転数の演算に供される。Next, the scanning device and the rocking drive device for the gimbal rocking mechanism will be described in detail with reference to FIG. FIG. 1 is a sectional view of a main part of a scanning device 2 mounted on a self-propelled vehicle 1, and the same reference numerals as in FIG. 2 indicate the same or equivalent parts. first,
The scanning device 2 will be explained. The mirror 4 is attached to one end 5a of the shaft of the motor 5 via a pedestal 4a. On the other hand, the other end 5b of the shaft of the motor 5 is connected to the shaft 7a of the encoder 7 by a connecting fitting 19. encoder 7
The output pulses are sent to a control device (not shown) and used to calculate the rotation angle and number of rotations of the mirror 4.
【0025】前記ミラー4の台座4aには吸着板34が
設けられている。この吸着板34は、磁性体、例えば鉄
で作られていて、電磁石16が付勢されることにより電
磁石16に吸着される。この吸着動作によって、電磁石
16が付勢された任意のタイミングでミラー4の停止位
置が固定される。内側リング部材14の下方には、ケー
シング3が取付けられている。このケーシング3の取付
手段は図示していないが、ボルト締めなど、周知の締結
手段を適宜使用すればよい。A suction plate 34 is provided on the pedestal 4a of the mirror 4. This attraction plate 34 is made of a magnetic material, for example iron, and is attracted to the electromagnet 16 when the electromagnet 16 is energized. By this attraction operation, the stop position of the mirror 4 is fixed at any timing when the electromagnet 16 is energized. A casing 3 is attached below the inner ring member 14. Although the means for attaching the casing 3 is not shown, any known fastening means such as bolt tightening may be used as appropriate.
【0026】次に、ジンバル揺動機構の揺動駆動装置に
ついて説明する。揺動駆動装置は自走車1の上面に設け
られている。自走車1の上面には1組のモータ15a,
15bが取付けられており、モータ15aの出力軸には
偏心ブロック21が、またモータ15bの出力軸には偏
心ブロック22がそれぞれ取付けられている。偏心ブロ
ック21,22の他端には、それぞれモータ15a,1
5bから所定量偏心した位置に軸21a,22aによっ
て球面軸受23,24が軸支され、この球面軸受23,
24はさらに連結ボルト26,28を介して球面軸受2
7,30に連結されている。Next, the swing drive device of the gimbal swing mechanism will be explained. The swing drive device is provided on the top surface of the self-propelled vehicle 1. On the top surface of the self-propelled vehicle 1, a set of motors 15a,
An eccentric block 21 is attached to the output shaft of the motor 15a, and an eccentric block 22 is attached to the output shaft of the motor 15b. The other ends of the eccentric blocks 21 and 22 are provided with motors 15a and 1, respectively.
Spherical bearings 23 and 24 are pivotally supported by shafts 21a and 22a at positions eccentrically eccentric from 5b by a predetermined amount, and these spherical bearings 23,
24 is further connected to the spherical bearing 2 via connecting bolts 26 and 28.
7,30.
【0027】そして、前記球面軸受27は、内側リング
部材14の、図示しない軸に支持され、球面軸受30は
、外側リング部材11に立設された軸29に支持されて
いる。前記偏心軸21aおよび偏心軸22aの偏心方向
は互いに90度ずらしてある。このように、偏心ブロッ
ク21の回転運動は、軸13および20を中心とする内
側リング部材14の上下方向の揺動運動に変換される。
同様に偏心ブロック22の回転運動は、軸12およびこ
れと対向する位置の軸(図示しない)を中心とする外側
リング部材11の上下方向の揺動運動に変換される。し
たがって、内側リング部材14および外側リング部材1
1の上下方向の揺動幅はモータ15a,15bの回転角
で決定され、また揺動速度はモータ15a,15bの回
転速度で決定される。The spherical bearing 27 is supported by a shaft (not shown) of the inner ring member 14, and the spherical bearing 30 is supported by a shaft 29 erected on the outer ring member 11. The eccentric directions of the eccentric shaft 21a and the eccentric shaft 22a are shifted from each other by 90 degrees. In this way, the rotational movement of the eccentric block 21 is converted into a vertical swinging movement of the inner ring member 14 about the axes 13 and 20. Similarly, the rotational movement of the eccentric block 22 is converted into a vertical swinging movement of the outer ring member 11 about the shaft 12 and an axis (not shown) located opposite thereto. Therefore, the inner ring member 14 and the outer ring member 1
1's vertical swing width is determined by the rotation angle of the motors 15a, 15b, and the swing speed is determined by the rotation speed of the motors 15a, 15b.
【0028】前記外側リング部材11および内側リング
部材14の揺動が合成されると、内側リング部材14に
取付けられている走査装置2のミラー4の回転中心軸8
が、両リング部材11および14のそれぞれの揺動中心
軸の交点を中心として、前記傾斜角度φだけ傾いた状態
で旋回する。換言すれば、この旋回による回転中心軸8
の軌跡は、前記交点を頂点とする円錐の側面(以下、単
に円錐という)となる。前記発光器および受光器を収容
しているケーシング3も、内側リング部材14の下面に
取付けられているので、この内側リング部材14と一体
となって揺動する。 なお、前記連結ボルト26,2
8の両端には互いに逆方向のねじが切られていて、これ
らの連結ボルト26,28を回転させると、連結ボルト
26は球面軸受23および27に対して進退し、連結ボ
ルト28は球面軸受24および30に対して進退する。
このようにして、ボルト26,28によって球面軸受2
7と球面軸受23との連結長さ、および球面軸受30と
球面軸受24との連結長さをそれぞれ調節することがで
きる。When the swinging movements of the outer ring member 11 and the inner ring member 14 are combined, the central axis of rotation 8 of the mirror 4 of the scanning device 2 attached to the inner ring member 14
The ring members 11 and 14 pivot about the intersection of their respective rocking center axes while being inclined by the inclination angle φ. In other words, the rotation center axis 8 due to this turning
The trajectory becomes the side surface of a cone (hereinafter simply referred to as a cone) having the intersection as its apex. Since the casing 3 housing the light emitter and the light receiver is also attached to the lower surface of the inner ring member 14, it swings together with the inner ring member 14. Note that the connection bolts 26, 2
8 are threaded in opposite directions, and when these connecting bolts 26 and 28 are rotated, the connecting bolt 26 moves forward and backward relative to the spherical bearings 23 and 27, and the connecting bolt 28 moves forward and backward relative to the spherical bearings 24. and advance and retreat against 30. In this way, the spherical bearing 2 is
The connection length between 7 and the spherical bearing 23 and the connection length between the spherical bearing 30 and the spherical bearing 24 can be adjusted.
【0029】本実施例では回転中心軸8の揺動軌跡がほ
ぼ円錐となるよう、つまり、外側リング部材11と内側
リング部材14のそれぞれの最大傾斜角度が同じになる
ように、モータ15a,15bの出力軸に対する偏心軸
21aおよび22aの偏心量をそれぞれ設定している。In this embodiment, the motors 15a and 15b are arranged so that the swing locus of the rotation center shaft 8 is approximately conical, that is, the maximum inclination angles of the outer ring member 11 and the inner ring member 14 are the same. The amount of eccentricity of the eccentric shafts 21a and 22a with respect to the output shaft is set respectively.
【0030】以上説明した揺動機構を駆動させて光ビー
ムを投射させると、ミラー4の回転中心軸8自体が円錐
を描いて回動する揺動走査が行われ、ミラー4の回転に
よって光跡で描かれる面(回転走査面)は一平面に固定
されず、揺動1サイクルの間は常に変化する。When the above-described swing mechanism is driven to project a light beam, a swing scan is performed in which the central axis of rotation 8 of the mirror 4 rotates in a conical manner, and the rotation of the mirror 4 creates a light trail. The surface drawn by (rotational scanning surface) is not fixed to one plane, but constantly changes during one swing cycle.
【0031】回転中心軸8が円錐を描いて1回転する周
期よりミラー4が1回転する周期の方を十分に短くする
ことにより、後述するようなきめの細かいピッチで走査
軌跡を描かせることができる。本実施例では、ミラー4
を2700rpmで回転させ、回転中心軸8を揺動させ
る軸22を90rpmで回転させるようにした。By making the period of one rotation of the mirror 4 sufficiently shorter than the period of one rotation of the central axis of rotation 8 drawing a cone, it is possible to draw a scanning locus at a fine pitch as described later. can. In this embodiment, mirror 4
was rotated at 2700 rpm, and the shaft 22 for swinging the rotation center shaft 8 was rotated at 90 rpm.
【0032】次に、図3および図4を参照して本実施例
の走査装置による光ビームの光跡について説明する。図
3には前記ミラー4を中心とした一定の半径を有する仮
想の円筒面に描かれた光跡をモデル化して示し、図4に
は光跡の要部を拡大して示した。なお、図3,図4は、
繁雑さを回避し、作図を容易にするためモデル化して示
してあるので、光跡の本数は実際より少なく記載してあ
る。Next, the light trail of the light beam produced by the scanning device of this embodiment will be explained with reference to FIGS. 3 and 4. FIG. 3 shows a modeled light trail drawn on a virtual cylindrical surface having a constant radius centered on the mirror 4, and FIG. 4 shows an enlarged view of the main part of the light trail. In addition, FIGS. 3 and 4 are
The number of light trails is shown as a model to avoid complexity and facilitate drawing, so the number of light trails is less than the actual number.
【0033】図3において、前記走査装置2から投射さ
れた光ビーム2Eは、ミラー4の回転中心軸8が円錐運
動をすることにより、前記想定された円筒面上に網目状
の光跡を描く。本実施例では、ミラー4の回転数を27
00rpm、回転中心軸8の揺動回数つまり軸22の回
転数を90rpmとしたので、回転中心軸8が円錐状に
1回転する間にミラー4自体は30回転する。したがっ
て、回転中心軸8が円錐を描いて1回転する間に、円筒
面上の任意の垂直線18を30本の光跡が横切る。In FIG. 3, the light beam 2E projected from the scanning device 2 draws a mesh-like light trail on the assumed cylindrical surface due to the conical movement of the rotation center axis 8 of the mirror 4. . In this embodiment, the number of rotations of the mirror 4 is set to 27.
00 rpm, and the number of swings of the rotation center shaft 8, that is, the rotation speed of the shaft 22, was set to 90 rpm, so that the mirror 4 itself rotates 30 times while the rotation center shaft 8 makes one conical rotation. Therefore, 30 light trails cross any vertical line 18 on the cylindrical surface during one rotation of the central axis of rotation 8 in a conical shape.
【0034】次に、前記垂直線18上に反射器を配設し
た場合、本実施例によれば揺動1サイクルの中でどれだ
け光ビームが反射器に照射されやすくなるかを説明する
。図4において、符号6Hで示すように自走車1と反射
器6とが近く、反射器6の高さ方向の寸法が光跡の揺動
幅BBに対して十分に長い場合は、30本の光跡がすべ
てこの反射器6を横切る。これに対し、符号6Lで示す
ように、自走車1と反射器6との距離が非常に長い場合
は、反射器6の高さ方向の寸法は光跡の揺動幅BBに対
して相対的に短くなる。このように、反射器6の高さ方
向寸法が相対的に短い場合であっても、光跡の垂直方向
の最大間隔Hが反射器6の高さ方向の寸法より相対的に
小さければ、回転中心軸8が円錐運動を1回行う間に少
なくとも1回は反射器6を光跡が横切る。Next, when the reflector is disposed on the vertical line 18, how easily the light beam is irradiated onto the reflector during one swing cycle according to this embodiment will be explained. In FIG. 4, when the self-propelled vehicle 1 and the reflector 6 are close to each other and the height dimension of the reflector 6 is sufficiently long with respect to the swing width BB of the light trail, as indicated by the reference numeral 6H, there are 30 lights. all light trails cross this reflector 6. On the other hand, when the distance between the self-propelled vehicle 1 and the reflector 6 is very long, as shown by the symbol 6L, the dimension in the height direction of the reflector 6 is relative to the swing width BB of the light trail. becomes shorter. In this way, even if the height dimension of the reflector 6 is relatively short, if the maximum vertical distance H between light trails is relatively smaller than the height dimension of the reflector 6, the rotation The light trail crosses the reflector 6 at least once during one conical movement of the central axis 8.
【0035】ところで、より高い確率で光ビームが反射
器6を横切るためには光跡を密にし、最大間隔Hが小さ
くなるようにすればよい。換言すれば、回転走査速度を
速くするか、揺動走査速度を遅くするか、揺動幅BBを
縮小するかのいずれかの調整をすれば、1回の揺動走査
の間(単位揺動サイクル中)における回動走査の回数つ
まり光跡の本数が多くなって最大間隔Hを小さくできる
。しかしながら、前記揺動幅BBの大きさを小さくする
と、自走車1が傾斜したような場合にこの揺動幅BB内
から反射器6が外れてしまい、光ビームを反射器6に照
射できないことがある。また、回転走査速度を高くする
にあたっては機械構造上の制約を十分に配慮する必要が
ある。By the way, in order for the light beam to cross the reflector 6 with a higher probability, the light traces should be made denser and the maximum distance H should be made smaller. In other words, if you make adjustments to increase the rotational scanning speed, slow down the oscillating scanning speed, or reduce the oscillating width BB, the The number of rotational scans (during a cycle), that is, the number of light trails, increases, and the maximum interval H can be reduced. However, if the swing width BB is made small, the reflector 6 will come off the swing width BB when the self-propelled vehicle 1 is tilted, and the light beam cannot be irradiated onto the reflector 6. There is. Furthermore, when increasing the rotational scanning speed, it is necessary to fully consider constraints on the mechanical structure.
【0036】さらに、採取した受光データを揺動走査に
同期させて揺動1サイクル毎に処理するように構成する
と処理システムの簡素化が図れるが、揺動走査速度を低
くせざるを得ない場合にはこの処理のタイミングセの間
隔が長くなって好ましくない場合がある。したがって、
本実施例では、これらの利点欠点を考慮して、前記揺動
幅、揺動走査速度、ならびに回転走査速度を常に最も望
ましい状態に設定できるようにした。Furthermore, if the collected light reception data is synchronized with the oscillation scanning and processed for each oscillation cycle, the processing system can be simplified, but if the oscillation scanning speed has to be lowered, In this case, the timing interval for this process becomes long, which may be undesirable. therefore,
In this embodiment, these advantages and disadvantages are taken into consideration so that the swing width, swing scanning speed, and rotational scanning speed can always be set to the most desirable state.
【0037】次に、上記の構成を有する走査装置2を搭
載した自走車1が、その走行領域内のどの位置にあるか
、またどの方向に走行しているかを検知するための基本
的原理を説明する。図5および図6は、自走車1の走行
領域を示す座標系における自走車1および反射器6a〜
6dの位置を示す図である。同図において、反射器6a
〜6dの配置位置、つまり基準点A,B,C,D,およ
び自走車1の位置T(Xp,Yp)は、基準点Bを原点
とし、基準点BおよびCを結ぶ直線をx軸とするx−y
座標系で表される。Next, the basic principle for detecting where the self-propelled vehicle 1 equipped with the scanning device 2 having the above configuration is located within its travel area and in which direction it is traveling. Explain. 5 and 6 show the self-propelled vehicle 1 and the reflectors 6a to 6a in the coordinate system showing the travel area of the self-propelled vehicle 1.
It is a figure which shows the position of 6d. In the figure, reflector 6a
~6d, that is, the reference points A, B, C, D, and the position T (Xp, Yp) of the self-propelled vehicle 1, the origin is the reference point B, and the x-axis is the straight line connecting the reference points B and C. x-y
Represented in a coordinate system.
【0038】図示のように、自走車1の位置Tは、三角
形ATBの外接円上に存在すると同時に、三角形BTC
の外接円上に存在する。したがって、自走車1の位置は
、これら2つの三角形の外接円QおよびPの交点を算出
することによって求められる。外接円QおよびPの2つ
の交点のうち、一方の交点は基準点Bつまり原点である
から、他方の交点が自走車1の位置ということになる。
このような原理に従って自走車1の位置を求める算出式
は、本出願人がすでに出願している特開平1−2874
15号および特開平1−316808号公報に詳細が示
されている。As shown, the position T of the self-propelled vehicle 1 is on the circumcircle of the triangle ATB and at the same time on the circumcircle of the triangle BTC.
Exists on the circumcircle of Therefore, the position of the self-propelled vehicle 1 is determined by calculating the intersection of the circumscribed circles Q and P of these two triangles. Since one of the two intersections of the circumscribed circles Q and P is the reference point B, that is, the origin, the other intersection is the position of the self-propelled vehicle 1. A calculation formula for determining the position of the self-propelled vehicle 1 according to this principle is disclosed in Japanese Patent Application Laid-Open No. 1-2874, which the applicant has already filed.
Details are shown in No. 15 and Japanese Unexamined Patent Publication No. 1-316808.
【0039】また、自走車1の進行方向は次式を用いて
算出される。図6において、自走車1の進行方向とx軸
とのなす角度をθf、進行方向を基準とした基準点Cの
方位角をθc、基準点Cのx座標をxc、自走車1のy
座標をYpとした場合、
θf=360°−tan−1{Yp/(xc−x)
}−θc………(1)となる。Furthermore, the traveling direction of the self-propelled vehicle 1 is calculated using the following equation. In FIG. 6, the angle between the traveling direction of the self-propelled vehicle 1 and the x-axis is θf, the azimuth angle of the reference point C with respect to the traveling direction is θc, the x-coordinate of the reference point C is xc, y
When the coordinate is Yp, θf=360°-tan-1{Yp/(xc-x)
}-θc (1).
【0040】次に、上記公報に記載された算出式および
上記算出式(1)によって求められた位置情報に基づい
て自走車1の走行方向を制御する操向制御について説明
する。図7は自走車1と基準点A〜Dとの位置関係を示
す図である。Next, a description will be given of steering control for controlling the running direction of the self-propelled vehicle 1 based on the position information obtained by the calculation formula described in the above publication and the calculation formula (1) above. FIG. 7 is a diagram showing the positional relationship between the self-propelled vehicle 1 and reference points A to D.
【0041】自走車1は基準点Bの近くのスタート位置
から走行を始め、予定の走行コース36を走行してホー
ムポジション63に戻るものとする。走行コースは間隔
Lを有して平行に設定された直進行程と、各直進行程を
つなぐ旋回行程とからなる。自走車1は直進行程を走行
した後、y座標がYtnまたはYtfに達した位置で、
操舵角度を一定の値に固定して旋回行程を走行し、隣接
する次の直進行程に移行する。そして、直進行程のx座
標が最終のx座標Xendを超過した場合、その直進行
程走行後、最終旋回行程を経てホームポジション63に
戻る。It is assumed that the self-propelled vehicle 1 starts traveling from a starting position near the reference point B, travels along the planned traveling course 36, and returns to the home position 63. The running course consists of straight strokes that are set in parallel with an interval L, and a turning stroke that connects the straight strokes. After the self-propelled vehicle 1 has traveled straight ahead, at the position where the y-coordinate reaches Ytn or Ytf,
The vehicle travels through a turning stroke with the steering angle fixed at a constant value, and then moves on to the next straight straight stroke. If the x-coordinate of the straight-line travel exceeds the final x-coordinate Xend, the vehicle returns to the home position 63 after the straight-line travel through the final turning stroke.
【0042】なお、図7においては、説明を簡単にする
ため、各基準点A,B,C,Dを、それらを結ぶ直線で
長方形が形成されるように配置した上で、直進行程は基
準点AとBとを結ぶ直線つまりy軸と平行にしたが、基
準点A〜Dを走行コースの周囲に配置してあれば、走行
コース36は任意に設定できる。In FIG. 7, in order to simplify the explanation, the reference points A, B, C, and D are arranged so that straight lines connecting them form a rectangle, and the straight line distance is Although the straight line connecting points A and B is parallel to the y-axis, the traveling course 36 can be arbitrarily set as long as the reference points A to D are arranged around the traveling course.
【0043】続いて、フローチャートを参照して制御手
順を説明する。この説明のために参照されるフローチャ
ートで使用される各種パラメータ(記号)の意味は次の
とおりである。θ(n)…受光信号に基づいて決定され
た方位角、θq(n)…予測方位角、Cg(i)…検出
ブロック別受光回数、Am(i)…検出ブロック別検出
方位角、Cp(n)…基準点nの受光回数、Ap[n,
I]…基準点nの受光方位角、As[n,I]…基準点
n検出時の揺動方向、Cm[n,I]…基準点n検出時
のミラー回転数カウンタ値、Aps(k)…受光回数が
しきい値以上の検出ブロックを代表する方位角、Aps
(n)…Aps(k)を小さい順にn=1〜4にセット
した方位角、i…検出ブロックの番号、j…受光回数が
第1のしきい値以上の検出ブロックの数、k…受光回数
が第2のしきい値以上の検出ブロックの数まず、操向制
御の基本となる反射光受光処理について説明する。走査
装置2から発射され、反射器6a〜6dで反射された光
ビームつまり反射光の受光処理は以下のとおり行われる
。Next, the control procedure will be explained with reference to a flowchart. The meanings of various parameters (symbols) used in the flowcharts referred to for this explanation are as follows. θ(n)...Azimuth determined based on the light reception signal, θq(n)...Predicted azimuth, Cg(i)...Number of light receptions for each detection block, Am(i)...Detection azimuth for each detection block, Cp( n)...Number of light reception at reference point n, Ap[n,
I]... Light receiving azimuth angle of reference point n, As [n, I]... Rocking direction when reference point n is detected, Cm [n, I]... Mirror rotation number counter value when reference point n is detected, Aps (k )...Azimuth representing the detection block whose number of light receptions is equal to or greater than the threshold value, Aps
(n)...Azimuth where Aps(k) is set to n=1 to 4 in descending order, i...Detection block number, j...Number of detection blocks whose number of light receptions is greater than or equal to the first threshold, k...Light reception Number of detection blocks whose number of times is equal to or greater than the second threshold value First, reflected light reception processing, which is the basis of steering control, will be explained. The light beam emitted from the scanning device 2 and reflected by the reflectors 6a to 6d, that is, the reflected light, is received and processed as follows.
【0044】図8は反射光受光処理の制御手順を示すフ
ローチャートである。ステップS100では、受光器に
よって光信号が検出されたか否かが判断される。光信号
が検出されたならば、ステップS101に進む。但し、
この時点では、検出された光信号は反射器6a〜6dか
らの反射光かどうかは識別できない。FIG. 8 is a flowchart showing the control procedure for the reflected light reception process. In step S100, it is determined whether an optical signal is detected by the light receiver. If an optical signal is detected, the process advances to step S101. however,
At this point, it is not possible to determine whether the detected optical signal is reflected light from the reflectors 6a to 6d.
【0045】ステップS101では、前回の処理後、ミ
ラー4が回転した角度が微小か否かによってチャタリン
グによる信号検出ではないかどうかが確認される。つま
り、ミラー4が微小角度しか回転しないうちに光信号が
複数検出された場合はチャタリングと判断して、後から
検出された光信号は無視する。チャタリングでなければ
、ステップS102に進む。In step S101, it is determined whether the signal detection is due to chattering, depending on whether the angle by which the mirror 4 was rotated after the previous processing is very small. That is, if a plurality of optical signals are detected before the mirror 4 rotates by only a small angle, it is determined that chattering is occurring, and optical signals detected later are ignored. If there is no chattering, the process advances to step S102.
【0046】ステップS102では、検出ブロック番号
を示す変数iに“0”をセットする。本実施例では、ミ
ラーの回転中心軸8が円錐状軌跡を描いて1回転する間
にミラー4は30回転する。すなわち回転中心軸8が円
錐状軌跡を描いて1回転する間に、回転走査が30回行
われるのである。この30回の回転走査によって同一の
反射器からの反射光を多数回受光する可能性がある。ほ
ぼ同一方向から受光器に入射した複数の光信号に関する
検出データは同一の反射器のデータとして1つのグルー
プにまとめて記憶するようにする。このグループを検出
ブロックという。したがって、予定の反射器6a〜6d
からの光だけが検出されたのであれば、この検出ブロッ
クの数は4つであり、設置されている反射器の数と一致
することになる。In step S102, a variable i indicating the detection block number is set to "0". In this embodiment, the mirror 4 rotates 30 times while the rotation center axis 8 of the mirror makes one rotation while drawing a conical locus. In other words, rotational scanning is performed 30 times while the rotation center shaft 8 makes one rotation while drawing a conical locus. Through these 30 rotational scans, there is a possibility that reflected light from the same reflector will be received many times. Detection data regarding a plurality of optical signals incident on the light receiver from substantially the same direction are stored together in one group as data for the same reflector. This group is called a detection block. Therefore, the intended reflectors 6a to 6d
If only the light from the source was detected, the number of detection blocks would be four, which would match the number of reflectors installed.
【0047】ステップS103では、前記検出ブロック
別の受光回数Cg(i)が“0”か否かを判断する。ス
テップS102でパラメータiに“0”がセットされた
ので、まず、検出ブロック番号“0”の検出ブロックで
の受光回数が“0”か否か、つまりこの検出ブロックで
最初に検出された光信号か否かが判断される。In step S103, it is determined whether the number of times Cg(i) of light reception for each detection block is "0" or not. Since the parameter i is set to "0" in step S102, first, it is determined whether or not the number of light receptions in the detection block with the detection block number "0" is "0", that is, the first optical signal detected in this detection block. It is determined whether or not.
【0048】最初の処理ではこの判断は肯定となってス
テップS106に進み、ミラー角つまり光を検出した方
位角が記憶される。検出ブロック(i)を代表する方位
角Am(i)として今回検出された方位角を記憶し、当
該検出ブロック(i)での光信号の受光回数Cg(i)
の値をインクリメントする。In the first process, this determination is affirmative, and the process proceeds to step S106, where the mirror angle, that is, the azimuth angle at which the light was detected, is stored. The currently detected azimuth is stored as the azimuth Am(i) representing the detection block (i), and the number of times the optical signal is received by the detection block (i) is calculated as Cg(i).
Increment the value of .
【0049】ステップS107では、基準点を識別する
カウンタの値nをクリアする。本実施例では、カウンタ
値“1”は基準点Aに、カウンタ値“2”は基準点Bに
、カウンタ値“3”は基準点Cに、カウンタ値“4”は
基準点Dにそれぞれ対応させてある。ステップS108
ではそのカウンタの値nをインクリメントする。In step S107, the value n of the counter for identifying the reference point is cleared. In this example, the counter value "1" corresponds to the reference point A, the counter value "2" corresponds to the reference point B, the counter value "3" corresponds to the reference point C, and the counter value "4" corresponds to the reference point D. I've let it happen. Step S108
Then, the value n of the counter is incremented.
【0050】ステップS109では、今回検出した方位
角が、後述のイニシャルポール識別処理や往路直進処理
で設定された予測方位角θq(n)とほぼ同一か否かが
判断される。すなわち、ステップS108でカウンタ値
nは“1”になっているので、このカウンタ値“1”に
対応する基準点Aの予測方位角に関し、これと検出方位
角とがほぼ一致するか否かが判断される。予測方位角θ
q(n)は、例えば、今回検出時の方位角に予測変化量
αを加算した値でも良いが、自走車1の移動量に対して
反射光の受光間隔が短いので今回の値と同値を予測方位
角としても実用上支障がないし、処理も簡単である。In step S109, it is determined whether or not the currently detected azimuth is substantially the same as the predicted azimuth θq(n) set in the initial pole identification process or the outgoing straight-on process, which will be described later. That is, since the counter value n is "1" in step S108, it is determined whether or not the predicted azimuth of the reference point A corresponding to this counter value "1" substantially matches the detected azimuth. be judged. Predicted azimuth angle θ
For example, q(n) may be a value obtained by adding the predicted change amount α to the azimuth at the time of current detection, but since the reception interval of reflected light is short relative to the amount of movement of the self-propelled vehicle 1, it should be the same value as the current value. There is no practical problem in using this as the predicted azimuth, and the processing is simple.
【0051】ステップS109の判断が否定の場合は、
ステップS110でカウンタ値nが“4”か否かが判別
される。ステップS110の判断が肯定になるまでステ
ップS108,S109の処理が繰返され、基準点A〜
Dのすべての予測方位角に関して、これと検出方位角と
がほぼ一致するか否かが判断される。[0051] If the determination in step S109 is negative,
In step S110, it is determined whether the counter value n is "4". The processes in steps S108 and S109 are repeated until the determination in step S110 becomes affirmative, and the reference points A to
Regarding all predicted azimuths of D, it is determined whether these and detected azimuths substantially match.
【0052】予測方位角θq(n)が検出方位角とほぼ
一致していれば、ステップS109からステップS11
1に進む。ステップS111では、予定の基準点を検出
したとして、カウンタ値nで示される基準点の受光回数
Cp(n)がインクリメントされる。さらに、ステップ
S111では、その基準点の検出方位角Ap[n,Cp
(n)]、ミラー4の回転中心軸8の傾斜方向つまり揺
動方向As[n,Cp(n)]、ならびにミラー4の回
転カウンタ値Cm[n,Cp(n)]を記憶する。ミラ
ー4の回転カウンタ値は、揺動方向がセンサ33の出力
信号に基づく予定の方向にある時を基準として、そこか
ら計数したミラー4の回転数が何回転目かを示す値であ
る。If the predicted azimuth θq(n) substantially matches the detected azimuth, steps S109 to S11
Go to 1. In step S111, assuming that the scheduled reference point has been detected, the number of times Cp(n) of light reception at the reference point indicated by the counter value n is incremented. Further, in step S111, the detected azimuth Ap[n, Cp
(n)], the tilting direction, that is, the swinging direction As[n, Cp(n)] of the rotation center axis 8 of the mirror 4, and the rotation counter value Cm[n, Cp(n)] of the mirror 4. The rotation counter value of the mirror 4 is a value indicating the number of rotations of the mirror 4 counted from the time when the rocking direction is in the predetermined direction based on the output signal of the sensor 33.
【0053】なお、ステップS103で、検出ブロック
(i)での受光回数Cg(i)が“0”でない、つまり
初めての受光でないと判断された場合は、ステップS1
04に進む。ステップS104では、検出方位角が、検
出ブロック(i)で先に受光した光信号の方位角Am(
i)とほぼ一致しているか否かが判断される。両者が一
致していればステップS106に進み、今回の検出方位
角で検出ブロック(i)の方位角Am(i)を更新する
。Note that if it is determined in step S103 that the number of light receptions Cg(i) in the detection block (i) is not "0", that is, it is not the first time that light is received, step S1
Proceed to 04. In step S104, the detection azimuth is the azimuth Am(
It is determined whether or not it substantially matches i). If the two match, the process advances to step S106, and the azimuth Am(i) of the detection block (i) is updated with the current detection azimuth.
【0054】また、ステップS104の判断が否定の場
合、つまり検出ブロック(i)で先に受光した光信号の
方位角Am(i)と今回検出された方位角とが一致して
いない場合は、他の検出ブロックからの光であると判断
してステップS105に進み、検出ブロック番号(i)
をインクリメントする。検出ブロック番号(i)をイン
クリメントした後、ステップS103にて、インクリメ
ントされた検出ブロック番号(i)について初めての受
光か否かの判断がなされる。If the determination in step S104 is negative, that is, if the azimuth Am(i) of the optical signal previously received by the detection block (i) does not match the azimuth detected this time, It is determined that the light is from another detection block, and the process proceeds to step S105, where the detection block number (i)
Increment. After the detection block number (i) is incremented, in step S103, it is determined whether or not the incremented detection block number (i) is the first light reception.
【0055】前記反射光受光処理によって記憶された受
光信号の方位角つまり基準点の方位角に基づいて後述の
ように自走車1の位置と進行方向とが演算され、操向制
御が行われる。Based on the azimuth of the light reception signal stored in the reflected light reception process, that is, the azimuth of the reference point, the position and traveling direction of the self-propelled vehicle 1 are calculated as described later, and steering control is performed. .
【0056】図9,図10は操向制御の全体を示すゼネ
ラルフローチャートである。図9において、ステップS
1ではモータ5およびモータ15aならびにモータ15
bを起動してミラー4を回転させると共に、その回転中
心軸8が円錐状の軌跡を描くようにジンバル揺動機構を
動作させる。ここでは、基準点A〜Dに設定された反射
器6a〜6dに確実に光ビームを照射させられるように
モータ15aおよび15bは低速で回転させる。FIGS. 9 and 10 are general flowcharts showing the entire steering control. In FIG. 9, step S
1, motor 5, motor 15a and motor 15
b is started to rotate the mirror 4, and the gimbal swing mechanism is operated so that the rotation center axis 8 draws a conical locus. Here, the motors 15a and 15b are rotated at low speed so that the reflectors 6a to 6d set at the reference points A to D are reliably irradiated with the light beam.
【0057】ステップS2では、基準点A〜Dつまり反
射器6a〜6dのイニシャル方位角を決定するイニシャ
ルポール識別処理を行う。この処理の詳細は図11に関
して後述する。In step S2, initial pole identification processing is performed to determine the initial azimuths of the reference points A to D, that is, the reflectors 6a to 6d. Details of this processing will be described later with reference to FIG.
【0058】ステップS3では、自走車1から基準点A
〜Dまでの各距離を測定して各基準点の位置つまり前記
x−y座標系における基準座標値を計算するポール位置
計測処理を行う。この処理の詳細は図15に関して後述
する。ステップS4では、イニシャルテーブル傾斜角設
定処理を行う。この処理では、自走車1の現在位置から
最も遠い距離にある基準点に光ビームを確実に照射でき
るような回転中心軸8の傾斜角度φおよびこの傾斜角度
φに応じた揺動速度を設定する。このイニシャルテーブ
ル傾斜角設定処理の詳細は図20に関して後述する。ス
テップS5では、後述する最大傾斜角度φmaxとして
“0”をセットする。In step S3, from the self-propelled vehicle 1 to the reference point A
A pole position measurement process is performed to measure each distance from ~D and calculate the position of each reference point, that is, the reference coordinate value in the x-y coordinate system. Details of this processing will be described later with reference to FIG. In step S4, initial table inclination angle setting processing is performed. In this process, an inclination angle φ of the rotation center shaft 8 and a swing speed corresponding to this inclination angle φ are set so that the light beam can be reliably irradiated to the reference point located at the farthest distance from the current position of the self-propelled vehicle 1. do. Details of this initial table inclination angle setting process will be described later with reference to FIG. In step S5, "0" is set as the maximum inclination angle φmax, which will be described later.
【0059】ステップS6では、反射光受光処理のデー
タを読込み、このデータに基づいて方位角を更新する。
この方位角更新処理の詳細は往路直進処理に関し、図1
6を参照して後述する。ステップS7では、ステップS
6で更新された基準点の方位角およびステップS3で算
出された基準点の座標値に基づき、現在の自走車1の位
置座標(Xp,Yp)を算出する。ステップS8では、
現在の自走車1のx座標Xpを第1番目の直進行程のx
座標Xrefとしてセットする。但し、この座標のセッ
ト値は、自走車1が走行開始位置にあると仮定した場合
の値である。In step S6, the data for the reflected light reception process is read, and the azimuth is updated based on this data. The details of this azimuth update process are related to the straight forward process and are shown in Figure 1.
This will be described later with reference to 6. In step S7, step S
Based on the azimuth of the reference point updated in step S6 and the coordinate value of the reference point calculated in step S3, the current position coordinates (Xp, Yp) of the self-propelled vehicle 1 are calculated. In step S8,
The current x-coordinate Xp of self-propelled vehicle 1 is
Set the coordinates as Xref. However, this coordinate set value is a value when it is assumed that the self-propelled vehicle 1 is at the travel start position.
【0060】ステップS9では、モータ5およびモータ
15aならびにモータ15bを所定の速度で高速回転さ
せてミラー4を回転および揺動させる。ステップS10
では、自走車1のエンジン回転を駆動輪に接続して走行
を開始させる。In step S9, the motor 5, motor 15a, and motor 15b are rotated at high speeds at predetermined speeds to rotate and swing the mirror 4. Step S10
Now, the engine rotation of the self-propelled vehicle 1 is connected to the drive wheels to start traveling.
【0061】図10のステップS11では、自走車1を
そのy座標値が大きくなる方向に直進行程を走行させる
往路直進処理を行う。この処理の詳細は図16に関して
後述する。ステップS12では、自走車1のy座標Yp
が予定のy座標Ytfより大きくなったか否かによって
第1番目の直進行程の走行を終了したか否かを判断する
。自走車1が直進行程の走行を終了したと判断されると
ステップS13に進む。ステップS13では、直進行程
のx座標Xrefに、隣接する直進行程までの距離Lを
加算して次の直進行程を設定する。ステップS14では
、旋回行程の走行を終了させる方位角を設定する右ター
ン解除角セット処理を行う。右ターン解除角セット処理
では次のステップで行われるUターン処理を終わらせる
タイミングを検出するための各基準点毎の方位角つまり
右ターン解除角を算出する。In step S11 of FIG. 10, an outbound straight-line process is performed in which the self-propelled vehicle 1 is caused to travel straight ahead in a direction in which the y-coordinate value increases. Details of this processing will be described later with reference to FIG. In step S12, the y-coordinate Yp of the self-propelled vehicle 1
It is determined whether or not the first straight-line travel has been completed based on whether or not the y-coordinate Ytf has become larger than the scheduled y-coordinate Ytf. When it is determined that the self-propelled vehicle 1 has finished traveling in a straight line, the process proceeds to step S13. In step S13, the distance L to the adjacent straight travel is added to the x-coordinate Xref of the straight travel to set the next straight travel. In step S14, right turn release angle setting processing is performed to set the azimuth at which the turning stroke is ended. In the right turn release angle setting process, the azimuth angle, that is, the right turn release angle, is calculated for each reference point in order to detect the timing to end the U-turn process performed in the next step.
【0062】ステップS15では、自走車1の操舵角を
予定値に固定して一定の旋回半径で右方向に旋回させ予
定の旋回行程上に自走車1を走行させるUターン処理を
行う。ステップS14および15の詳細な処理内容は、
それぞれ図17および図19に関して後述する。In step S15, a U-turn process is performed in which the steering angle of the self-propelled vehicle 1 is fixed to a predetermined value and the self-propelled vehicle 1 is caused to turn to the right at a constant turning radius and travel on the predetermined turning stroke. The detailed processing contents of steps S14 and 15 are as follows.
These will be described later with reference to FIGS. 17 and 19, respectively.
【0063】ステップS16では、自走車1から見た方
位角が予定の右ターン解除角に達した基準点の数を計数
する解除カウンタ(図19の処理で計数される)の値が
“1”を超過しているか否かを判断する。この判断が肯
定の場合は、旋回行程の走行を終了したと判断してステ
ップS17に進む。In step S16, the value of the release counter (counted in the process of FIG. 19) that counts the number of reference points where the azimuth seen from the self-propelled vehicle 1 has reached the scheduled right turn release angle is "1". ” is exceeded. If this determination is affirmative, it is determined that the turning stroke has ended, and the process proceeds to step S17.
【0064】ステップS17では、自走車1をそのy座
標値が小さくなる方向に直進行程を走行させる復路直進
処理を行う。この復路直進処理はステップS11の往路
直進処理と同様であり、詳細の説明は省略する。ステッ
プS18では、自走車1のy座標Ypが予定のy座標Y
tnより小さいか否かによって第2番目の直進行程の走
行を終了したか否かを判断する。ステップS19では、
直進行程のx座標Xrefが走行終了予定地点のx座標
Xendを超過したか否かを判断する。ステップS19
の判断が否定の場合は、ステップS20に進んで次の直
進行程を設定する。ステップS21では、左方向旋回行
程の走行を終了させる方位角を設定する左ターン解除角
セット処理を行う。この処理は後述の解除角のセット値
のみが異なる他は前記右ターン解除角セット処理と同様
である。ステップS22では、Uターン処理を行う。こ
の処理はステップS15のUターン処理と同様である。
ステップS23では、解除カウンタの値が“1”を超過
しているか否かを判断する。この判断が肯定の場合は、
旋回行程の走行を終了したと判断してステップS11に
戻る。[0064] In step S17, a return trip process is performed in which the self-propelled vehicle 1 is caused to travel straight in a direction in which the y-coordinate value thereof becomes smaller. This straight-on return process is similar to the straight-on outward process in step S11, and detailed explanation will be omitted. In step S18, the y-coordinate Yp of the self-propelled vehicle 1 is changed to the scheduled y-coordinate Yp.
It is determined whether or not the second straight travel has been completed depending on whether or not the value is smaller than tn. In step S19,
It is determined whether the x-coordinate Xref of the straight travel distance exceeds the x-coordinate Xend of the scheduled end point of the run. Step S19
If the determination is negative, the process advances to step S20 to set the next straight stroke. In step S21, left turn release angle setting processing is performed to set the azimuth angle at which the leftward turning stroke is ended. This process is similar to the right turn release angle setting process, except that the set value of the release angle, which will be described later, is different. In step S22, a U-turn process is performed. This process is similar to the U-turn process in step S15. In step S23, it is determined whether the value of the release counter exceeds "1". If this judgment is affirmative,
It is determined that the turning stroke has ended, and the process returns to step S11.
【0065】また、ステップS19の判断が肯定の場合
は、ステップS24に進む。ステップS19の判断が肯
定の場合はすべての直進行程の走行を終了した場合であ
り、ステップS24に進み、最終の旋回行程における解
除角をセットする処理を行う。この処理は右ターン解除
角セットと同様に処理される。Further, if the determination in step S19 is affirmative, the process advances to step S24. If the determination in step S19 is affirmative, this means that all straight-line travel has been completed, and the process proceeds to step S24, where a process is performed to set the release angle for the final turning stroke. This process is performed in the same way as the right turn release angle setting.
【0066】ステップS25ではUターン処理を行い、
ステップS26では、解除カウンタの値が1を超過して
いるか否かを判断する。ステップS27では、ホームポ
ジション63に戻る直進行程を走行させる処理を行う。
この処理は往路直進処理と同様である。[0066] In step S25, a U-turn process is performed,
In step S26, it is determined whether the value of the release counter exceeds 1 or not. In step S27, processing is performed to make the vehicle travel straight back to the home position 63. This process is similar to the straight forward process.
【0067】ステップS28では、自走車1のx座標X
pがホームポジション63のx座標Xhomeより小さ
くなったか否かを判断する。この判断が肯定ならば、自
走車1がホームポジション63に戻ったと判断して処理
を終える。In step S28, the x-coordinate of the self-propelled vehicle 1
It is determined whether p has become smaller than the x-coordinate Xhome of the home position 63. If this determination is affirmative, it is determined that the self-propelled vehicle 1 has returned to the home position 63, and the process ends.
【0068】次に、前記ステップS2のイニシャルポー
ル識別処理について詳述する。図11はイニシャルポー
ル識別処理のフローチャートである。同図において、ス
テップS120では、揺動方向が“0°”になったか否
か、つまりモータ15aのステップ角に基づいて光ビー
ムの揺動走査方向が予定の基準方向にあるか否かを判断
する。揺動走査方向が予定の基準方向にあったと判断さ
れると、ステップS121に進む。ステップS121で
は、反射光受光処理によって得られたデータを記憶する
メモリ領域のデータをクリアする。Next, the initial pole identification process in step S2 will be described in detail. FIG. 11 is a flowchart of the initial pole identification process. In the figure, in step S120, it is determined whether the swing direction has reached "0°", that is, whether the swing scanning direction of the light beam is in the predetermined reference direction based on the step angle of the motor 15a. do. If it is determined that the swing scanning direction is in the predetermined reference direction, the process advances to step S121. In step S121, the data in the memory area that stores the data obtained by the reflected light reception process is cleared.
【0069】ステップS122では、図8に示した反射
光受光処理を行う。このイニシャルポール識別処理にお
ける反射光受光処理では、この処理以前に予測方位角が
決定されていないので、図8に関して説明した反射光受
光処理のうちの、ステップS100〜S106に相当す
る処理が行われる。In step S122, the reflected light reception process shown in FIG. 8 is performed. In the reflected light reception process in this initial pole identification process, since the predicted azimuth has not been determined before this process, processes corresponding to steps S100 to S106 of the reflected light reception process described with reference to FIG. 8 are performed. .
【0070】ステップS123では、再び揺動方向が“
0°”か否かを判断する。つまり回転中心軸8が円錐を
描く揺動の1サイクルが終了したか否かが判断される。
1サイクルの終了までは反射光受光処理を続け、1サイ
クルが終了すると、ステップS124に進む。In step S123, the rocking direction is changed again to "
0°". In other words, it is determined whether one cycle of the rotation center axis 8 oscillating in a conical manner has been completed.The reflected light reception process continues until the end of one cycle, and one cycle is completed. When the process ends, the process advances to step S124.
【0071】ステップS124では、基準点の選択処理
(ポール選択処理)を行う。この選択処理では、反射光
受光処理において検出された検出ブロックのうち受光回
数Cg(i)の多い4つの検出ブロックを選択し、その
検出ブロックを代表する検出方位角Am(i)を方位角
の小さい順にAps(n)にセットする。なお本実施例
では基準点はA,B,C,Dの4本なのでn=1〜4で
ある。図8の反射光受光処理のフローチャートで示した
ように、検出ブロックを代表する検出方位角Am(i)
は、その検出ブロックで検出された方位角の最新データ
である。このように最新データを使用することにより、
この方位角を記憶するメモリの記憶容量を節約すること
ができる。In step S124, reference point selection processing (pole selection processing) is performed. In this selection process, four detection blocks with a large number of light receptions Cg(i) are selected from among the detection blocks detected in the reflected light reception process, and the detection azimuth Am(i) representing the detection block is set as the azimuth angle. Set Aps(n) in descending order. In this embodiment, there are four reference points A, B, C, and D, so n=1 to 4. As shown in the flowchart of reflected light reception processing in FIG. 8, the detection azimuth Am(i) representing the detection block
is the latest azimuth data detected by that detection block. By using the latest data in this way,
The storage capacity of the memory for storing this azimuth angle can be saved.
【0072】また、ステップS124では、次のステッ
プS125における判断の材料となるポール選択モード
「1」〜「3」の決定も行われる。ポール選択処理は、
さらに図12に関して詳細に述べる。Furthermore, in step S124, the pole selection modes "1" to "3" are determined, which will serve as the basis for the determination in the next step S125. The pole selection process is
Further details will be given with respect to FIG.
【0073】ステップS125では、ポール選択処理で
決定されたポール選択モードが「1」〜「3」のいずれ
であるかを判別する。ポール選択モードが「1」の場合
は、4つの基準点のすべてが識別でき、その方位角を検
出できたとしてステップS126に進む。ステップS1
26では、基準点の方位角θ(n)として、ステップS
124の処理で得られた方位角Aps(n)をセットす
る。また、ポール選択モードが「2」の場合は、受光回
数Cg(i)が所定値に達していないため、ステップS
122に戻って反射光受光処理を継続する。さらに、ポ
ール選択モードが「3」の場合は、予定外の反射物体が
あったりして、受光回数Cg(i)が所定値以上の検出
ブロックが5つ以上の場合である。この場合は検出ブロ
ックの中から基準点を特定することができなかったとし
てステップS121に戻り、最初からこのイニシャルポ
ール識別処理をやり直す。In step S125, it is determined whether the pole selection mode determined in the pole selection process is one of "1" to "3". If the pole selection mode is "1", it is assumed that all four reference points have been identified and their azimuths have been detected, and the process advances to step S126. Step S1
26, the azimuth angle θ(n) of the reference point is set as step S
The azimuth Aps(n) obtained in step 124 is set. Furthermore, if the pole selection mode is "2", the number of light receptions Cg(i) has not reached the predetermined value, so step S
The process returns to step 122 to continue the reflected light reception process. Furthermore, when the pole selection mode is "3", there are five or more detection blocks in which the number of light receptions Cg(i) is equal to or greater than a predetermined value due to an unexpected reflective object. In this case, it is determined that the reference point could not be identified from within the detection block, and the process returns to step S121, and the initial pole identification process is restarted from the beginning.
【0074】次に、このイニシャルポール識別処理にお
ける反射光受光処理で得られた受光データの例を示す。
イニシャルポール識別処理では、回転中心軸8が1回揺
動する間、つまりミラー4が30回転する間に記憶され
た受光データに基づいて基準点の識別処理を行う。図2
3はミラー4が30回転する間に蓄積された受光データ
の例を示す図である。Next, an example of light reception data obtained in the reflected light reception process in this initial pole identification process will be shown. In the initial pole identification process, a reference point identification process is performed based on the light reception data stored while the rotation center shaft 8 swings once, that is, while the mirror 4 rotates 30 times. Figure 2
3 is a diagram showing an example of received light data accumulated while the mirror 4 rotates 30 times.
【0075】同図において、縦軸は検出ブロック(i)
の受光回数Cg(i)であり、横軸は各検出ブロック(
i)の方位角Am(i)である。図示のように、検出ブ
ロックの数は7つ(i=0〜6)あったとすれば、これ
はミラー4が30回転する間に7方向から光信号が受光
されたことを示す。受光回数Cg(i)が複数回の検出
ブロックでは、方位角Am(i)は前述のように最新の
検出データである。なお、検出ブロックの番号iが必ず
しも方位角の小さい順に並んでいないのは、受光した順
に番号が付されているためである。In the figure, the vertical axis represents the detection block (i)
The horizontal axis is the number of light receptions Cg(i) of each detection block (
i) is the azimuth angle Am(i). As shown in the figure, if there are seven detection blocks (i=0 to 6), this means that optical signals were received from seven directions while the mirror 4 rotated 30 times. In a detection block in which the number of light receptions Cg(i) is plural, the azimuth Am(i) is the latest detection data as described above. Note that the detection block numbers i are not necessarily arranged in order of decreasing azimuth angle because the numbers are assigned in the order of light reception.
【0076】この受光データを参照しながら前記ポール
選択処理の詳細を説明する。図12はポール選択処理の
フローチャートである。このポール選択処理では、受光
回数Cg(i)が予定のしきい値に達している検出ブロ
ックを抽出し、抽出した検出ブロックの数が予定の基準
点の数つまり“4”と一致しているかどうかを判別する
。一致していればその検出ブロックの受光データを予定
の基準点A〜Dに関するデータであると決定する。また
、抽出された検出ブロックの数が多い場合は基準点を特
定できないと判断して改めてデータの採取を行い、また
、抽出された検出ブロックの数が少ない場合はさらにデ
ータの採取を継続する。The details of the pole selection process will be explained with reference to this light reception data. FIG. 12 is a flowchart of the pole selection process. In this pole selection process, detection blocks whose number of light receptions Cg(i) has reached a predetermined threshold value are extracted, and whether the number of extracted detection blocks matches the predetermined number of reference points, that is, "4"? determine whether If they match, the light reception data of that detection block is determined to be data related to the scheduled reference points A to D. Furthermore, if the number of extracted detection blocks is large, it is determined that the reference point cannot be identified and data is collected again, and if the number of extracted detection blocks is small, data collection is continued.
【0077】本実施例では、しきい値を“3”と“5”
の2段階設定した。そして、受光回数Cg(i)が第1
のしきい値“3”に達している検出ブロックの数はパラ
メータjで記憶し、第2のしきい値“5”に達している
検出ブロックの数はパラメータkで記憶するようにして
いる。そして、このパラメータj,kに基づき、基準点
を識別してよいか否かを判断するのである。In this embodiment, the threshold values are set to "3" and "5".
Two stages were set. Then, the number of light receptions Cg(i) is the first
The number of detected blocks that have reached the second threshold value "3" is stored as a parameter j, and the number of detected blocks that have reached the second threshold value "5" is stored as a parameter k. Based on these parameters j and k, it is determined whether or not the reference point can be identified.
【0078】まず、ステップS130では、反射光受光
処理で得られたデータつまり光検出方位角Am(i)と
反射光の受光回数Cg(i)とを読込む。First, in step S130, data obtained in the reflected light reception process, that is, the light detection azimuth Am(i) and the number of times Cg(i) of the reflected light is received are read.
【0079】ステップS131では、パラメータi,j
,kをクリアする。ステップS132では、当該検出ブ
ロック(i)が、反射光の受光回数Cg(i)が3回よ
り多い検出ブロックか否かを判断する。受光回数Cg(
i)が3回より多い検出ブロックであったならば、ステ
ップS133に進み、受光回数Cg(i)が3回以上あ
った検出ブロックの数を示すパラメータjをインクリメ
ントする。In step S131, parameters i, j
, k is cleared. In step S132, it is determined whether the detection block (i) has received reflected light more than three times Cg(i). Number of light reception Cg(
If i) is a detection block that has been detected more than three times, the process proceeds to step S133, and a parameter j indicating the number of detection blocks for which the number of light receptions Cg(i) has been three or more times is incremented.
【0080】ステップS134では、受光回数Cg(i
)が5回より多い検出ブロックか否かを判断する。
受光回数が5回より多い検出ブロックであったならば、
ステップS135に進み、受光回数が5回以上あった検
出ブロックの数を示すパラメータkをインクリメントす
る。In step S134, the number of light receptions Cg(i
) is detected more than 5 times. If the detection block receives light more than 5 times,
Proceeding to step S135, a parameter k indicating the number of detection blocks for which light has been received five or more times is incremented.
【0081】ステップS136では、パラメータkの値
が“4”以上か否かを判断する。受光回数Cg(i)が
5回を超える検出ブロックが4つすなわち予定の基準点
の総数以上あったか否かを判別するのである。受光回数
Cg(i)が5回を超える検出ブロックが4つ以下の場
合は、ステップS137において、受光回数Cg(i)
が5回以上あった検出ブロックを代表する検出方位角A
m(i)を予定の基準点のうちの1つの方位角Aps(
k)として記憶する。In step S136, it is determined whether the value of parameter k is "4" or more. It is determined whether there are four detection blocks in which the number of light receptions Cg(i) exceeds 5, that is, there are more than the total number of scheduled reference points. If the number of detection blocks for which the number of light receptions Cg(i) exceeds 5 is four or less, in step S137, the number of light receptions Cg(i)
Detection azimuth A representing a detection block where 5 or more times
Let m(i) be the azimuth angle Aps (
k).
【0082】ステップS138では、検出ブロックを示
す番号iをインクリメントする。ステップS139では
、受光回数Cg(i)が“0”か否かを判断する。受光
回数Cg(i)が“0”でなければ、まだ、受光回数が
記憶されている検出ブロックがあると判断して、ステッ
プS132に戻る。受光回数Cg(i)が“0”であれ
ば、もう、検出ブロックはないと判断してステップS1
40に進む。In step S138, the number i indicating the detected block is incremented. In step S139, it is determined whether the number of light receptions Cg(i) is "0". If the number of light receptions Cg(i) is not "0", it is determined that there is still a detection block in which the number of light receptions is stored, and the process returns to step S132. If the number of light receptions Cg(i) is "0", it is determined that there are no more detection blocks and the process proceeds to step S1.
Proceed to 40.
【0083】ステップS140では、パラメータj,k
が共に“4”か否か、つまり受光回数が3回以上あった
検出ブロックの数と、5回以上あった検出ブロックの数
とが共に4つであったか否かを判断する。[0083] In step S140, parameters j, k
It is determined whether or not both are "4", that is, whether the number of detection blocks that received light three times or more and the number of detection blocks that received light five or more times were both four.
【0084】ステップS140が肯定の場合は、ステッ
プS141に進み、ステップS137で記憶された検出
方位角Aps(k)を、小さい順に方位角Aps(1)
〜Aps(4)としてセットする。ステップS142で
は、ポール選択モードを「1」とする。If step S140 is affirmative, the process advances to step S141, where the detected azimuths Aps(k) stored in step S137 are sorted in ascending order of azimuths Aps(1).
~Aps(4). In step S142, the pole selection mode is set to "1".
【0085】前記ステップS140の判断が否定の場合
は、ステップS143に進んでパラメータjが“4”以
上か否か、つまり受光回数が3回以上あった検出ブロッ
クが4つ以上あったか否かを検出する。ステップS14
3が否定の場合は、ステップS144でポール選択モー
ドを「2」とし、肯定の場合は、ステップS145でポ
ール選択モードを「3」とする。決定されたこれらのポ
ール選択モードに従い、図11のイニシャルポール識別
処理におけるステップS125の判断を行う。If the determination in step S140 is negative, the process proceeds to step S143, where it is determined whether the parameter j is "4" or more, that is, whether there are four or more detection blocks that have received light three or more times. do. Step S14
If 3 is negative, the pole selection mode is set to "2" in step S144, and if affirmative, the pole selection mode is set to "3" in step S145. According to these determined pole selection modes, the determination in step S125 in the initial pole identification process of FIG. 11 is performed.
【0086】例えば、図23に示した受光データの例で
は、受光回数Cg(i)が5回を超えている検出ブロッ
ク(番号i=0,2,3,5)の方位角が予定の基準点
の方位角であると識別される。For example, in the example of the light reception data shown in FIG. 23, the azimuth of the detection block (number i = 0, 2, 3, 5) for which the number of light receptions Cg(i) exceeds 5 is the scheduled standard. is identified as the azimuth of the point.
【0087】次に、図9のステップS3に示したポール
位置計測処理に必要となる基準点の座標を決定する処理
について説明する。まず、そのために各基準点に光ビー
ムを照射させる確率の高い回転中心軸8の傾きの方向(
揺動方向)を決定するポール捕捉揺動方向決定処理を説
明する。このポール捕捉揺動方向決定処理の概要は次の
とおりである。Next, the process of determining the coordinates of the reference point necessary for the pole position measurement process shown in step S3 of FIG. 9 will be described. First, for this purpose, the direction of inclination of the rotation center axis 8 (
The pole capture swing direction determination process for determining the swing direction (swing direction) will be explained. The outline of this pole capture swing direction determination process is as follows.
【0088】図24に、反射光受光処理で検出された基
準点A(n=1)に関する受光データの例を示す。同図
には、揺動の1サイクルつまりミラー4が30回転する
間に検出した受光データを示す。本実施例では、後述の
ように回転中心軸8が1回転する間、すなわちミラー4
が30回転する間の受光データを取込むようにしている
。同図において揺動方向とは、モータ15aおよび15
bの回転角に基づいて計算された回転中心軸8の傾斜方
向であり、あらかじめ設定された基準方向からの角度を
表すデータである。FIG. 24 shows an example of light reception data regarding the reference point A (n=1) detected in the reflected light reception process. The figure shows light reception data detected during one cycle of rocking, that is, when the mirror 4 rotates 30 times. In this embodiment, as will be described later, the mirror 4 is
Light reception data is captured during 30 rotations. In the figure, the swinging direction refers to the motors 15a and 15.
This is the inclination direction of the rotation center axis 8 calculated based on the rotation angle of b, and is data representing an angle from a preset reference direction.
【0089】パラメータCmは図8の反射光受光処理の
フローチャートで示したように、揺動方向が“0°”の
位置から計数を開始し、基準点nを検出したときの回転
カウンタ値を示すものであり、ここでは、ミラー4が3
0回転する間の受光データを採取しているのでパラメー
タCmの最大は“30”である。パラメータIは基準点
nを検出したときの揺動方向の記憶順を示す番号である
。すなわちこの記憶順を示す番号Iの最大値が基準点n
の受光回数Cp(n)となる。As shown in the flowchart of the reflected light reception process in FIG. 8, the parameter Cm indicates the rotation counter value when counting starts from the position where the swing direction is "0°" and the reference point n is detected. , and here mirror 4 is 3
Since the light reception data is collected during zero rotation, the maximum parameter Cm is "30". Parameter I is a number indicating the storage order of the rocking directions when the reference point n is detected. In other words, the maximum value of number I indicating this storage order is the reference point n
The number of times of light reception is Cp(n).
【0090】このポール捕捉揺動方向決定処理では、ミ
ラー4の回転走査の1サイクル毎に基準点nを連続して
検出したグループを探索する。例えば図24において番
号Iが“1”および“2”、ならびに“3”〜“6”で
示される揺動方向はそれぞれ1つのグループのデータと
して取扱う。このようなグループのうちの連続検出回数
が最も多いグループにおける揺動方向の範囲を検出する
。そして範囲の中位値を算出する。そして、得られた揺
動方向の中位値が、基準点nに高い確率で光ビームを照
射させることができる揺動方向であると決定する。この
中位値は同一グループのすべての揺動方向の平均値でも
よいし、最大値および最小値の平均値でもよい。本実施
例では最大値および最小値の平均値を算出するようにし
た。In this pole capture swing direction determination process, a group in which the reference point n is successively detected is searched for every cycle of rotational scanning of the mirror 4. For example, in FIG. 24, the rocking directions indicated by numbers I "1" and "2" and "3" to "6" are each handled as one group of data. Among such groups, the range of the swing direction in the group with the largest number of consecutive detections is detected. Then, calculate the median value of the range. Then, the obtained median value of the swing direction is determined to be the swing direction that allows the light beam to be irradiated to the reference point n with a high probability. This median value may be the average value of all rocking directions of the same group, or may be the average value of the maximum value and minimum value. In this embodiment, the average value of the maximum value and the minimum value is calculated.
【0091】例えば図24においては、基準点n検出時
にカウンタ値Cmが連続しているのは番号Iが“1”〜
“2”のときと、“3”〜“6”のときであり、このう
ち、番号Iが“3”〜“6”のときが連続回数が多い。
したがって、番号Iが“3”と“6”とにおける揺動方
向の値を平均して基準点(ポール)捕捉揺動方向を決定
する。平均された値は(108.6°+144.4°)
/2=126.5°である。For example, in FIG. 24, when the reference point n is detected, the counter values Cm are consecutive when the number I is "1" to "1".
When the number I is "2" and when it is "3" to "6", and among these, when the number I is "3" to "6", the number of consecutive times is large. Therefore, the reference point (pole) capture swing direction is determined by averaging the swing direction values for numbers I of "3" and "6". The average value is (108.6° + 144.4°)
/2=126.5°.
【0092】図13,図14はポール捕捉揺動方向決定
処理のフローチャートである。図13において、ステッ
プS150では、揺動方向が“0°”になったか否かを
判断する。揺動方向が“0°”になったと判断されると
、ステップS151に進む。ステップS151では、反
射光受光処理によって得られたデータを記憶するメモリ
領域のデータをクリアする。ステップS152では、反
射光受光処理を行う。この処理は図8に関して説明した
とおりである。FIGS. 13 and 14 are flowcharts of the pole capture swing direction determination process. In FIG. 13, in step S150, it is determined whether the swing direction has reached "0°". If it is determined that the rocking direction has become "0°", the process advances to step S151. In step S151, data in a memory area that stores data obtained by the reflected light reception process is cleared. In step S152, reflected light reception processing is performed. This process is as described with respect to FIG.
【0093】ステップS153では、揺動方向が“0°
”になったのが2回目か否か、すなわち揺動走査の1サ
イクルが行われたかどうかを判断する。この判断によっ
て、回転中心軸8が円錐を描く揺動が1サイクル終了し
たと判断されるとステップS154に進む。すなわち、
回転中心軸8が円錐を描く揺動が1回行われる間反射光
受光処理を行いその間の受光データを蓄積する。[0093] In step S153, the swing direction is "0°".
” is the second time, that is, whether one cycle of oscillation scanning has been performed. By this judgment, it is determined that one cycle of oscillation in which the rotation center axis 8 draws a cone has been completed. Then, the process advances to step S154. That is,
While the central axis of rotation 8 is oscillated in a conical manner once, reflected light reception processing is performed and the light reception data during that time is accumulated.
【0094】ステップS154では、反射光受光処理の
ステップS111(図8)で得られたデータを読込む。
ステップS155では、基準点の識別カウンタnをクリ
アする。ステップS156では、前記カウンタnをイン
クリメントする。ステップS157では、前記カウンタ
nで示される基準点(基準点n)を検出した回数を示す
パラメータCp(n)が“0”か否かを判別してその基
準点nが検出されたか否かを判断する。この判断が肯定
の場合は、基準点nが検出されなかったとしてステップ
S150に戻り、再び受光データを採取する。In step S154, the data obtained in step S111 (FIG. 8) of the reflected light reception process is read. In step S155, the reference point identification counter n is cleared. In step S156, the counter n is incremented. In step S157, it is determined whether the parameter Cp(n) indicating the number of times the reference point (reference point n) indicated by the counter n has been detected is "0", and whether or not the reference point n has been detected is determined. to decide. If this determination is affirmative, it is assumed that the reference point n has not been detected, and the process returns to step S150 to collect light reception data again.
【0095】ステップS157が否定の場合は、ステッ
プS158に進み、パラメータIに“1”をセットする
。このパラメータIは上述のように基準点nが検出でき
た揺動方向を記憶した順を識別するための番号である。If step S157 is negative, the process advances to step S158, and the parameter I is set to "1". This parameter I is a number for identifying the order in which the swing directions in which the reference point n was detected are stored, as described above.
【0096】ステップS159では、パラメータKおよ
びeをクリアする。このパラメータKはミラー4が1回
転する毎に同じ方位からの受光信号を検出した場合、つ
まり同じ方位からの光信号を毎回連続して検出した場合
に、その連続回数の最大値つまり最大連続検出回数を示
す。また、パラメータeは揺動方向の記憶番号Iのうち
、特に連続して光信号を検出した時の最後の揺動方向の
記憶番号を示す。In step S159, parameters K and e are cleared. This parameter K is the maximum value of the number of consecutive detections, that is, the maximum continuous detection when a light reception signal from the same direction is detected every time the mirror 4 rotates, that is, when a light signal from the same direction is detected continuously every time. Indicates the number of times. Moreover, the parameter e indicates the storage number of the last oscillation direction when optical signals are continuously detected among the oscillation direction storage numbers I.
【0097】図14のステップS160では、受光信号
の連続回数を計数するカウンタの値Jに“1”をセット
する。ステップS161では、基準点nを検出した回数
を示すパラメータCp(n)が揺動方向の記憶数字を示
すパラメータIと同じか否かを判別することによって、
記憶した受光データのすべてがチェックされたか否かを
判断する。通常は受光データは複数存在しており、ステ
ップS158では番号Iを“1”としたので、この場合
はステップS161の判断は否定となり、受光データの
すべてをチェックし終っていないと判断されてステップ
S162に進む。In step S160 of FIG. 14, a value J of a counter that counts the number of consecutive light reception signals is set to "1". In step S161, by determining whether the parameter Cp(n) indicating the number of times the reference point n has been detected is the same as the parameter I indicating the memorized number of the rocking direction,
It is determined whether all of the stored light reception data has been checked. Normally, there is a plurality of received light data, and the number I was set to "1" in step S158, so in this case, the determination in step S161 is negative, and it is determined that all of the received light data has not been checked, so step Proceed to S162.
【0098】ステップS162では、前記ミラー4の回
転カウンタ値Cmに関し、今回検出時のカウンタ値Cm
(n,I+1)が前回検出時のカウンタ値Cm(n,I
)に“1”が加算された値になっているか否かを判別す
る。すなわち、カウンタ値Cmの連続を判断してミラー
4の回転毎に連続して受光信号が検出されているか否か
を判断する。この判断が肯定の場合は、ステップS16
3に進み、カウンタJおよびパラメータIの値をインク
リメントする。In step S162, regarding the rotation counter value Cm of the mirror 4, the counter value Cm at the time of current detection is determined.
(n, I+1) is the counter value Cm(n, I
) is the value obtained by adding “1” to the value. That is, it is determined whether or not the light reception signal is continuously detected each time the mirror 4 rotates by determining the continuity of the counter value Cm. If this judgment is affirmative, step S16
Proceed to step 3 and increment the values of counter J and parameter I.
【0099】一方、ステップS162が否定の場合は、
ステップS164に進み、カウンタ値JがパラメータK
より大きいか、つまり今回の連続検出回数がそれまでに
記憶されていた連続回数より大きいか否かが判断される
。この判断が肯定の場合はステップS165に進み、最
大連続検出回数Kを今回の連続検出回数Jで更新し、連
続受光信号検出時の最後の揺動方向の記憶数字を示すパ
ラメータeを今回の揺動方向の記憶番号Iで更新する。
ステップS166では、揺動方向の記憶数字Iをインク
リメントする。On the other hand, if step S162 is negative,
Proceeding to step S164, the counter value J is the parameter K.
In other words, it is determined whether the current number of consecutive detections is greater than the previously stored number of consecutive detections. If this judgment is affirmative, the process advances to step S165, where the maximum number of continuous detections K is updated with the current number of consecutive detections J, and the parameter e indicating the memorized number of the last swing direction when detecting the continuous light reception signal is set to the current number of swings. Update with storage number I of moving direction. In step S166, the memory number I for the swing direction is incremented.
【0100】一方、ステップS161が肯定の場合、つ
まりすべての受光データのチェックが終了したと判断さ
れたならば、ステップS167に進む。ステップS16
7およびS168は、前記ステップS164およびS1
65と同じ処理であるので説明は省略する。On the other hand, if step S161 is affirmative, that is, if it is determined that all received light data has been checked, the process advances to step S167. Step S16
7 and S168 are steps S164 and S1
Since this is the same process as 65, the explanation will be omitted.
【0101】ステップS169では、最大連続検出回数
が発生した場合の最初の揺動方向記憶番号を算出してパ
ラメータIとする。ステップS170では、揺動方向の
最小値minとして反射光受光処理で検出された揺動方
向As(n,I)、つまり最大連続検出回数が発生した
場合の最初の記憶番号に対応して記憶されている揺動方
向のデータをセットする。In step S169, the first rocking direction storage number when the maximum number of consecutive detections occurs is calculated and set as parameter I. In step S170, the rocking direction As(n, I) detected in the reflected light reception process is stored as the minimum value min of the rocking direction, corresponding to the first storage number when the maximum number of consecutive detections occurs. Set the data of the rotation direction.
【0102】また、最大値maxとしてAs(n,e)
つまり最大連続検出回数が発生した場合の最後の記憶番
号に対応して記憶されている揺動方向のデータをセット
する。 ステップS171では、前記最大値maxと
最小値minとの平均値を演算して基準点n捕捉揺動方
向Asc(n)として記憶する。この基準点n捕捉揺動
方向Asc(n)は、基準点nが高い確率で検出できる
揺動方向ということになる。 ステップS172では
、カウンタ値nが“4”か否かによって、すべての基準
点に関して基準点n捕捉揺動方向を決定する処理を終了
したか否かを判断する。ステップS172が否定の場合
は図13のステップS156に進む。[0102] Also, As(n, e) as the maximum value max
That is, the data of the rocking direction stored in correspondence with the last storage number when the maximum number of consecutive detections has occurred is set. In step S171, the average value of the maximum value max and the minimum value min is calculated and stored as the reference point n capture swing direction Asc(n). This reference point n capturing swing direction Asc(n) is a swing direction in which the reference point n can be detected with a high probability. In step S172, depending on whether the counter value n is "4" or not, it is determined whether the process of determining the reference point n capture swing direction for all reference points has been completed. If step S172 is negative, the process advances to step S156 in FIG.
【0103】次に、上述の基準点n捕捉揺動方向(基準
点捕捉揺動方向)に従って基準点の座標を決定するため
の処理を説明する。図15は基準点の座標を決定するポ
ール位置計測処理のフローチャートである。Next, a process for determining the coordinates of the reference point according to the above-described reference point n capture swing direction (reference point capture swing direction) will be described. FIG. 15 is a flowchart of the pole position measurement process for determining the coordinates of the reference point.
【0104】同図において、ステップS180では、前
記ポール捕捉揺動方向決定処理を行う。ステップS18
1では、基準点を識別するためのカウンタnの値をクリ
アし、ステップS182で、そのカウンタ値をインクリ
メントする。In the figure, in step S180, the pole capture swing direction determination process is performed. Step S18
In step S182, the value of a counter n for identifying a reference point is cleared, and in step S182, the counter value is incremented.
【0105】ステップS183では、前記ポール捕捉揺
動方向決定処理で求められた揺動方向Asc(n)を基
準点n捕捉揺動方向TG−SWとしてセットし、前記イ
ニシャルポール識別処理で求められた方位角θ(n)を
基準点n捕捉方位角TG−MLとしてセットする。In step S183, the swing direction Asc(n) obtained in the pole capture swing direction determination process is set as the reference point n capture swing direction TG-SW, and Set the azimuth θ(n) as the reference point n capture azimuth TG-ML.
【0106】ステップS184では、揺動用モータ15
aおよび15bを駆動して揺動方向を前記基準点n捕捉
揺動方向TG−SWに合わせるための揺動位置制御を行
う。そして、揺動方向を前記基準点n捕捉揺動方向TG
−SWに合わせ後、その状態で回転中心軸8の傾きを固
定する。回転中心軸8の傾きを固定した結果、光跡によ
って描かれる回転走査面は1平面に固定される。[0106] In step S184, the swing motor 15
A and 15b are driven to perform swing position control for aligning the swing direction with the reference point n capture swing direction TG-SW. Then, the rocking direction is set to the reference point n and the rocking direction TG.
- After adjusting to SW, the inclination of the rotation center axis 8 is fixed in that state. As a result of fixing the inclination of the rotation center axis 8, the rotation scanning plane drawn by the light trace is fixed to one plane.
【0107】ステップS185では、回転走査面を固定
した結果、基準点nが連続して毎回検出されているか否
かを判断する。この判断が否定の場合はセンサの故障な
どの異常があると判断し、異常を知らせる表示をしたり
、警報を発したりして(ステップS186のセンサフェ
ール処理)当該処理を停止する。[0107] In step S185, it is determined whether or not the reference point n is continuously detected each time as a result of fixing the rotating scanning plane. If this determination is negative, it is determined that there is an abnormality such as a sensor failure, a display indicating the abnormality is displayed or an alarm is issued (sensor fail processing in step S186), and the processing is stopped.
【0108】一方、基準点nが連続して毎回検出されて
いて、ステップS185の判断が肯定の場合は、ステッ
プS187に進む。ステップS187では、前記基準点
n捕捉方位角TG−MLに基準点方位角を調整するミラ
ー位置制御処理を行う。このミラー位置制御処理ではミ
ラー回転用モータ5の回転を検出するエンコーダ7の値
が基準点n捕捉方位角TG−MLと略一致するまでモー
タ5を回転させる。そしてエンコーダ7の値が基準点n
捕捉方位角TG−MLと略一致したときにモータ5を停
止させる。さらに、その停止位置でモータ5が停止状態
を維持できるように電磁石16を付勢する。これによっ
て吸着板34が電磁石16に吸着され、モータ5の停止
位置つまりミラー4の停止位置が固定される。On the other hand, if the reference point n is continuously detected each time and the determination in step S185 is affirmative, the process advances to step S187. In step S187, mirror position control processing is performed to adjust the reference point azimuth to the reference point n acquisition azimuth TG-ML. In this mirror position control process, the motor 5 is rotated until the value of the encoder 7 that detects the rotation of the mirror rotation motor 5 substantially matches the reference point n capture azimuth TG-ML. Then, the value of encoder 7 is the reference point n
The motor 5 is stopped when the captured azimuth substantially matches the captured azimuth TG-ML. Further, the electromagnet 16 is energized so that the motor 5 can remain stopped at the stopped position. As a result, the suction plate 34 is attracted to the electromagnet 16, and the stop position of the motor 5, that is, the stop position of the mirror 4 is fixed.
【0109】ステップS188では、基準点nからの反
射光を3秒以上検出する。もちろんこの時間は3秒に限
らず、ミラー4の方位が確実に固定されたかどうかが確
認できる時間であればよい。In step S188, the reflected light from the reference point n is detected for 3 seconds or more. Of course, this time is not limited to 3 seconds, and may be any time that allows it to be confirmed whether the orientation of the mirror 4 is reliably fixed.
【0110】ステップS189では、基準点nからの反
射光の検出結果に基づいて自走車1と基準点nとの距離
を測定する。これは、発光器から出た光ビームと受光器
で検出された反射光の位相差によって演算する。ステッ
プS190では、基準点を識別するためのカウンタ値n
が“4”か否か、つまりすべての基準点について自走車
1からの距離を測定し終わったか否かを判断する。ステ
ップS191では、検出された各基準点と自走車1との
距離、および前記イニシャルポール識別処理によって求
められた方位角に基づき、自走車1を原点とする任意の
座標系、例えば自走車1を原点とし基準点A方向をx軸
の正方向とする座標系上での各基準点A〜Dの座標[X
(n),Y(n)]を演算する。In step S189, the distance between the self-propelled vehicle 1 and the reference point n is measured based on the detection result of the reflected light from the reference point n. This is calculated based on the phase difference between the light beam emitted from the light emitter and the reflected light detected by the light receiver. In step S190, the counter value n for identifying the reference point is
It is determined whether or not is "4", that is, whether the distances from the self-propelled vehicle 1 have been measured for all reference points. In step S191, based on the distance between each detected reference point and the self-propelled vehicle 1, and the azimuth obtained by the initial pole identification process, an arbitrary coordinate system having the self-propelled vehicle 1 as the origin, for example, a self-propelled The coordinates of each reference point A to D on a coordinate system with car 1 as the origin and reference point A direction as the positive
(n), Y(n)].
【0111】ステップS192では、前記座標を基準点
Bを原点とする座標系上での座標[x(n),y(n)
]に変換する。[0111] In step S192, the coordinates are expressed as coordinates [x(n), y(n)] on the coordinate system with reference point B as the origin.
] Convert to
【0112】続いて、前記ステップS8の往路直進処理
の詳細を説明する。図16は往路直進処理のフローチャ
ートである。同図において、ステップS200では、反
射光受光処理を行う。ステップS201では、揺動方向
が“0°”か否かによって、回転中心軸8が円錐を描く
揺動を1サイクル終えたかどうかを判断する。この揺動
の1サイクルを終了するまで反射光の受光処理を行い、
ステップS202で、この反射光受光処理によって得ら
れたデータを読込む。ステップS203では、反射光受
光処理によって得られた方位角Am(i)の中から予測
方位角θqに最も近いものを抽出し、各基準点の方位角
Ac(n)に格納する。ステップS204では、カウン
タ値nをクリアし、ステップS205ではその値をイン
クリメントする。ステップS206では、基準点nの方
位角θ(n)および予測方位角θq(n)に前記方位角
Ac(n)をセットする。[0112] Next, the details of the outbound straight forward processing in step S8 will be explained. FIG. 16 is a flowchart of the straight forward processing. In the figure, in step S200, reflected light reception processing is performed. In step S201, it is determined whether the central axis of rotation 8 has completed one cycle of swinging in a conical shape, depending on whether the swinging direction is "0°" or not. The reflected light is received and processed until one cycle of this oscillation is completed.
In step S202, data obtained by this reflected light reception processing is read. In step S203, the one closest to the predicted azimuth θq is extracted from among the azimuths Am(i) obtained by the reflected light reception process, and stored in the azimuth Ac(n) of each reference point. In step S204, the counter value n is cleared, and in step S205, the value is incremented. In step S206, the azimuth Ac(n) is set to the azimuth θ(n) of the reference point n and the predicted azimuth θq(n).
【0113】すべての基準点A〜Dについて方位角θ(
n)と予測方位角θq(n)との更新が終了したという
ことがステップS207で判断されたならば、ステップ
S208に進み、反射光受光処理によって得られたデー
タをクリアしてステップS209に進む。[0113] For all reference points A to D, the azimuth angle θ(
If it is determined in step S207 that the update of n) and predicted azimuth θq(n) has been completed, the process proceeds to step S208, where the data obtained by the reflected light reception process is cleared, and the process proceeds to step S209. .
【0114】ステップS209では、自走車1から最も
遠い基準点に対して光ビームを確実に照射できる前記回
転中心軸8の傾斜角度φを設定し、その傾斜角度φに対
応する揺動速度を計算するテーブル傾斜角設定処理を行
う。この処理の詳細は、図21に関して詳述する。[0114] In step S209, the inclination angle φ of the rotation center shaft 8 that can reliably irradiate the light beam to the reference point farthest from the self-propelled vehicle 1 is set, and the rocking speed corresponding to the inclination angle φ is determined. Performs table tilt angle setting processing to be calculated. The details of this process will be explained in detail with respect to FIG.
【0115】ステップS210では、ステップS206
セットされた各基準点A〜Dについての方位角θ(n)
と、前記ポール位置計測処理で得られた角基準点A〜D
の位置情報に基づいて、図5,図6ですでに説明したよ
うにして自走車1の位置(Xp,Yp)および進行方向
θfを算出する。ステップS211では、設定された走
行コース(直進行程)に対するx座標Xrefと自走車
1のx座標Xpとのずれ量ΔX、および進行方向θfが
直進状態からずれている角度Δθを算出する。ステップ
S212では、前記ずれ量ΔXおよびずれ角度Δθを修
正するように操向制御を行う。[0115] In step S210, step S206
Azimuth angle θ(n) for each set reference point A to D
and the corner reference points A to D obtained by the pole position measurement process.
Based on the position information, the position (Xp, Yp) and traveling direction θf of the self-propelled vehicle 1 are calculated as already explained in FIGS. 5 and 6. In step S211, the amount of deviation ΔX between the x-coordinate Xref and the x-coordinate Xp of the self-propelled vehicle 1 with respect to the set travel course (straight travel distance) and the angle Δθ by which the traveling direction θf deviates from the straight-ahead state are calculated. In step S212, steering control is performed to correct the deviation amount ΔX and the deviation angle Δθ.
【0116】前記往路直進処理によって自走車1がy座
標Ytfに達した時点で次の直進行程に移行するため旋
回を開始する。そして、予定の位置まで旋回した後、今
度は復路の直進を開始する必要がある。この旋回終了位
置は自走車1から見た各基準点A〜Dの方位角が予定の
ターン解除角になった位置とする。本実施例では、少な
くとも1つの基準点に関する方位角が、決定されたター
ン解除角付近に達したことが2回検出された位置を旋回
終了位置であると判断することにしている。[0116] When the self-propelled vehicle 1 reaches the y-coordinate Ytf in the straight forward process, it starts turning in order to proceed to the next straight process. Then, after turning to the planned position, it is necessary to start going straight on the return trip. This turning end position is the position where the azimuth angle of each reference point A to D as seen from the self-propelled vehicle 1 becomes the planned turn release angle. In this embodiment, the position where it is detected twice that the azimuth angle with respect to at least one reference point has reached the vicinity of the determined turn release angle is determined to be the turning end position.
【0117】図17は、往路直進処理後のターン解除角
を算出してセットする右ターン解除角セット処理のフロ
ーチャートである。FIG. 17 is a flowchart of the right turn release angle setting process for calculating and setting the turn release angle after the straight forward process.
【0118】同図において、ステップS220では、旋
回行程の走行を終了する位置の座標(x,y)としてそ
れぞれXref,Ytfをセットする。ステップS22
1では、カウンタ値nをクリアし、ステップS222で
は、このカウンタ値nをインクリメントする。In the figure, in step S220, Xref and Ytf are respectively set as the coordinates (x, y) of the position where the turning stroke ends. Step S22
1, the counter value n is cleared, and in step S222, the counter value n is incremented.
【0119】ステップS223では、旋回行程の走行を
終了する位置(x,y)において、進行方向の角度θf
がx軸を基準にして270°となった時(図18参照)
の基準点nの方位角θt(n)を算出し、ターン解除角
とする。このターン解除角θt(n)には旋回に移行す
るのが遅れないように少し早めに解除するためのオフセ
ット量を付加する。[0119] In step S223, at the position (x, y) where the turning stroke ends, the angle θf in the traveling direction is
is 270° with respect to the x-axis (see Figure 18)
The azimuth angle θt(n) of the reference point n is calculated and used as the turn release angle. An offset amount is added to this turn release angle θt(n) in order to release the turn a little earlier so that there is no delay in transitioning to a turn.
【0120】ステップS224では、基準点A〜Dのす
べてに関してターン解除角がセットされたか否かをカウ
ンタ値nが“4”になったか否かで判断する。この判断
が肯定ならばステップS225に進む。ステップS22
5では、解除カウンタの値をクリアする。この解除カウ
ンタは方位角がターン解除角に達した基準点の数を計数
するもので、この解除カウンタの値によって旋回を終了
するか否かの判定(図10のステップS13)を行う。
ステップS226では、自走車1の操舵角を予定の右タ
ーン用操舵角に固定する。In step S224, it is determined whether the turn release angles have been set for all of the reference points A to D, based on whether the counter value n has become "4". If this determination is affirmative, the process advances to step S225. Step S22
In step 5, the value of the release counter is cleared. This release counter counts the number of reference points whose azimuth reaches the turn release angle, and it is determined whether or not to end the turn based on the value of this release counter (step S13 in FIG. 10). In step S226, the steering angle of the self-propelled vehicle 1 is fixed to the planned right turn steering angle.
【0121】次に、前記ステップS12の旋回行程走行
中の処理(Uターン処理)について説明する。図19は
Uターン処理のフローチャートである。同図において、
ステップS230では、図8に関して詳述した反射光受
光処理を行う。ステップS231では、揺動方向が“0
°”か否かを判断する。この判断が肯定の場合は、ステ
ップS232で方位角更新処理を行う。方位角更新処理
は図16のステップS202〜209と同様の処理であ
り、予測方位角θq(n)および方位角θ(n)の更新
、およびテーブル傾斜角設定処理を行う。Next, the processing during the turning stroke (U-turn processing) in step S12 will be explained. FIG. 19 is a flowchart of the U-turn process. In the same figure,
In step S230, the reflected light reception process described in detail with respect to FIG. 8 is performed. In step S231, the swing direction is "0".
°”. If this determination is affirmative, the azimuth update process is performed in step S232. The azimuth update process is similar to steps S202 to 209 in FIG. 16, and the predicted azimuth θq (n) and azimuth angle θ(n), and table inclination angle setting processing is performed.
【0122】なお、この方位角更新処理は、回転中心軸
8の揺動が1サイクル終了した時点のみで行われるので
方位角が単位時間で大きく変化する旋回行程では検出精
度が低下してターンの解除が大きく遅れるおそれがある
。したがって、本実施例では、揺動方向が90°、18
0°、ならびに270°の時点(ステップS233が肯
定となった場合)においても方位角を更新するようにし
ている。ステップS234の方位角更新サブ処理では、
ステップS232の方位角更新処理とほぼ同じであるが
、予測方位角θq(n)の更新およびテーブル傾斜角設
定処理は行わない点、ならびに反射光受光処理のデータ
をリセットしない点が異なる。[0122] This azimuth update process is performed only when one cycle of rotation of the rotation center shaft 8 is completed, so in a turning process where the azimuth changes greatly per unit time, the detection accuracy decreases and the turning process is delayed. There is a risk that the cancellation will be significantly delayed. Therefore, in this embodiment, the rocking directions are 90° and 18°.
The azimuth angle is also updated at 0° and 270° (if step S233 is affirmative). In the azimuth update sub-process in step S234,
This process is almost the same as the azimuth update process in step S232, except that the predicted azimuth angle θq(n) is not updated and the table tilt angle setting process is not performed, and the data for the reflected light reception process is not reset.
【0123】ステップS235では、基準点識別カウン
タnの値をクリアし、ステップS236では、その値を
インクリメントする。ステップS237では、検出方位
角θ(n)が前記右ターン解除角θt(n)とほぼ一致
したか否かが判断される。この判断が肯定の場合はステ
ップS238に進んで解除カウンタの値をインクリメン
トする。In step S235, the value of the reference point identification counter n is cleared, and in step S236, the value is incremented. In step S237, it is determined whether the detected azimuth angle θ(n) substantially matches the right turn release angle θt(n). If this determination is affirmative, the process advances to step S238 and the value of the release counter is incremented.
【0124】ステップS239では基準点識別カウンタ
nの値が“4”か否かを判断する。これによって、4つ
の基準点のすべてに関してその方位角θ(n)が右ター
ン解除角θt(n)と一致しているか否かの判定を終了
したか否かの判断を行う。In step S239, it is determined whether the value of the reference point identification counter n is "4". As a result, it is determined whether or not the azimuth angle θ(n) of all four reference points matches the right turn release angle θt(n).
【0125】4つの基準点のすべてに関して判断を終了
すると、図10のステップS13,S20,S23で解
除カウンタの判別が行われる。そして、その値が“1”
以上であると、それぞれ復路直進処理、往路直進処理、
ホーム直進処理に進む。[0125] When the judgments regarding all four reference points are completed, the release counter is judged in steps S13, S20, and S23 in Fig. 10. And the value is “1”
If the above is the case, the return trip straight process, the outbound trip straight process,
Go straight to the platform.
【0126】図10のステップS21における最終ター
ン解除角のセットは、自走車1の座標が(Xend,Y
tn)にあって、自走車1の進行方向が、x軸を基準と
して180°にある場合、つまりホームポジション63
側に向いている場合のターン解除角がセットされる。こ
の処理はターン解除角をセットするときの位置とそのセ
ットデータ値が違うことを除いては前記右ターン解除角
セットと実質上同一である。左ターン解除角の算出式は
図17に記載した。The setting of the final turn release angle in step S21 in FIG. 10 is performed when the coordinates of the self-propelled vehicle 1 are
tn), the traveling direction of the self-propelled vehicle 1 is at 180° with respect to the x-axis, that is, the home position 63
Sets the turn release angle when facing to the side. This process is substantially the same as the right turn release angle setting described above, except that the position when setting the turn release angle and the set data value are different. The formula for calculating the left turn release angle is shown in FIG. 17.
【0127】また、最終ターン解除角に基づいて最終の
旋回行程の走行を終えた後、ホームポジション63に戻
るまでの直進行程の走行、ホーム直進処理は図16の往
路直進処理と走行コースからずれ量計算のときのコース
設定値が違うだけで、その他は同様である。[0127] Furthermore, after completing the final turning stroke based on the final turn release angle, the straight-line process until returning to the home position 63, and the home straight process, deviate from the outbound straight process and the running course in FIG. The only difference is the course setting value when calculating the amount, and the other things are the same.
【0128】続いて、前記往路直進処理やUターン処理
等で行われるイニシャルテーブル傾斜角設定処理および
テーブル傾斜角設定処理について説明する。まず、最も
遠い基準点に光ビームを確実に照射させることができる
傾斜角度φを求めるための算出式を示す。図22は、こ
の算出式に用いられる符号の説明図である。同図におい
て、走査装置の基準面SFに対する初期設定傾斜角度を
符号φTで示し、基準点の方位角をθで示す。同図から
分かるように、 l=ab・tanφT, ab=
r・sinθ,l=r・tanφxから、r・tanφ
x=r・sinθ・tanφTとなり、最も遠い基準点
の方向での傾斜角度φxは次式のとおりとなる。
φx=tan−1(sinθ・tanφT)………
……(2)この傾斜角度φxに対し、光ビーム照射の確
実性を増大させるための予定角度Δφを付加したものを
設定傾斜角度φとする。Next, the initial table inclination angle setting process and the table inclination angle setting process that are performed in the straight forward process, the U-turn process, etc. will be explained. First, a calculation formula for determining the inclination angle φ that can reliably irradiate the light beam to the farthest reference point will be shown. FIG. 22 is an explanatory diagram of the symbols used in this calculation formula. In the figure, the initially set inclination angle of the scanning device with respect to the reference plane SF is indicated by the symbol φT, and the azimuth angle of the reference point is indicated by θ. As can be seen from the figure, l=ab・tanφT, ab=
From r・sinθ, l=r・tanφx, r・tanφ
x=r·sinθ·tanφT, and the inclination angle φx in the direction of the farthest reference point is as follows. φx=tan-1(sinθ・tanφT)……
(2) The set inclination angle φ is defined as the addition of a planned angle Δφ for increasing the reliability of light beam irradiation to this inclination angle φx.
【0129】
φ=φx+Δφ……………(3)
上記の算出式(2)および(3)を使用して行われるイ
ニシャルテーブル設定処理のフローチャートを図20に
示す。同図において、ステップS300では、自走車1
から最も遠い距離にある基準点の方位角θ(n)と、初
期設定傾斜角度φTとを読込む。この最も遠い距離にあ
る基準点は、ポール位置計測処理で検出された各基準点
の位置と自走車1との位置関係によって選択される。ま
た、初期設定傾斜角度φTはあらかじめ(初期に)設定
されていた傾斜角度である。φ=φx+Δφ (3) FIG. 20 shows a flowchart of the initial table setting process performed using the above calculation formulas (2) and (3). In the figure, in step S300, the self-propelled vehicle 1
The azimuth angle θ(n) of the reference point located at the farthest distance from the reference point and the initial setting inclination angle φT are read. The farthest reference point is selected based on the positional relationship between the self-propelled vehicle 1 and the position of each reference point detected in the pole position measurement process. Further, the initial setting inclination angle φT is an inclination angle that is set in advance (initially).
【0130】ステップS301では、前記最も遠い距離
にある基準点方向での傾斜角度φxを前記算出式(2)
を利用して算出する。ステップS302では、前記算
出式(3)を使用して、傾斜角度φxに予定角度Δφを
付加した設定傾斜角度φを算出する。[0130] In step S301, the inclination angle φx in the direction of the reference point located at the farthest distance is calculated using the formula (2).
Calculate using. In step S302, the set inclination angle φ is calculated by adding the planned angle Δφ to the inclination angle φx using the calculation formula (3).
【0131】ステップS303では、算出された設定傾
斜角度φで揺動走査を行うためのモータ15a,15b
の回転角をセットする。モータ15a,15bはこのセ
ットされた回転角を得るためのパルスを供給されて所定
の回転角だけ駆動される。[0131] In step S303, the motors 15a and 15b are activated to perform swing scanning at the calculated set inclination angle φ.
Set the rotation angle. The motors 15a and 15b are supplied with pulses to obtain the set rotation angle and are driven by a predetermined rotation angle.
【0132】ステップS304では、前記設定傾斜角度
φに対応する揺動速度つまりモータ15a,15bの速
度を設定する。ここで、前記設定傾斜角度φに対応する
揺動速度は、設定傾斜角度φが大きくなるに従って小さ
く、設定傾斜角度φが小さくなるに従って大きくなるよ
うに、計算式に従って計算した値を設定してもよいし、
計算によらず、あらかじめ設定傾斜角度φに対応させた
揺動速度をROMテーブルに記憶させておき、計算され
た設定傾斜角度φをアドレスとして、揺動速度を読出す
ようにしてもよい。[0132] In step S304, the swinging speed, that is, the speed of the motors 15a and 15b, corresponding to the set inclination angle φ is set. Here, the rocking speed corresponding to the set tilt angle φ may be set to a value calculated according to the formula so that it decreases as the set tilt angle φ increases and increases as the set tilt angle φ decreases. Good and
Instead of calculation, the swing speed corresponding to the set tilt angle φ may be stored in a ROM table in advance, and the swing speed may be read using the calculated set tilt angle φ as an address.
【0133】次に、図21を参照してテーブル傾斜角設
定処理を説明する。同図において、ステップS310で
は基準点を見失ったか否かを判断する。この判断は反射
光を受光したかどうかによって行う。基準点を見失って
いなければステップS311に進み、このテーブル傾斜
角設定処理の回数を計数するカウンタの値Nが“20”
か否かを判断する。Next, table inclination angle setting processing will be explained with reference to FIG. In the figure, in step S310, it is determined whether the reference point has been lost. This judgment is made based on whether reflected light is received. If the reference point has not been lost, the process advances to step S311, and the value N of the counter that counts the number of times this table tilt angle setting process is performed is "20".
Determine whether or not.
【0134】カウンタ値が“20”になるまでは、ステ
ップS312に進む。ステップS312では、算出式(
2)を使用して自走車1から最も遠い距離にある基準点
に対する傾斜角度φxを算出する。なお、自走車1から
最も遠い距離にある基準点は、自走車1の現在位置およ
び各基準点A〜Dの座標から算出して判断する。[0134] Until the counter value reaches "20", the process advances to step S312. In step S312, the calculation formula (
2) is used to calculate the inclination angle φx with respect to the reference point located at the farthest distance from the self-propelled vehicle 1. Note that the reference point located at the farthest distance from the self-propelled vehicle 1 is calculated and determined from the current position of the self-propelled vehicle 1 and the coordinates of each of the reference points A to D.
【0135】ステップS313では、算出された傾斜角
度φxが最大傾斜角φmaxより大きいか否かを判断す
る。最大傾斜角度φmaxは、このテーブル傾斜角設定
処理を過去20回までの最大の傾斜角度である。ステッ
プS313の判断が肯定ならばステップS314に進ん
で今回計算された傾斜角度φxを最大傾斜角度φmax
として記憶する。[0135] In step S313, it is determined whether the calculated inclination angle φx is larger than the maximum inclination angle φmax. The maximum inclination angle φmax is the maximum inclination angle in which this table inclination angle setting process has been performed up to 20 times in the past. If the determination in step S313 is affirmative, the process proceeds to step S314, where the currently calculated inclination angle φx is set to the maximum inclination angle φmax.
be memorized as .
【0136】ステップS313の判断が否定の場合は、
最大傾斜角度φmaxを今回の傾斜角度φxで更新する
処理(ステップS314)は行わず、ステップS315
に進んでカウンタ値Nをインクリメントする。[0136] If the judgment in step S313 is negative,
The process of updating the maximum inclination angle φmax with the current inclination angle φx (step S314) is not performed, and step S315
Then, the counter value N is incremented.
【0137】また、カウンタ値nが“20”になったな
らば、ステップS311からステップS316に進み、
過去20回の処理における最大傾斜角度φmaxに予定
の角度幅Δφを付加した値を設定傾斜角度φとして確定
する。そして、ステップS317で、設定傾斜角度φだ
け回転中心軸8を傾斜させて揺動するためのモータ15
a,15bの回転角をセットする。[0137] Furthermore, if the counter value n becomes "20", the process advances from step S311 to step S316,
The value obtained by adding the planned angular width Δφ to the maximum inclination angle φmax in the past 20 processes is determined as the set inclination angle φ. Then, in step S317, the motor 15 for tilting and swinging the rotation center shaft 8 by the set tilt angle φ
Set the rotation angles of a and 15b.
【0138】ステップS318では、カウンタ値をクリ
アし、ステップS319では最大傾斜角度φmaxをク
リアする。ステップS320では、前記設定傾斜角度φ
に対応する揺動速度つまりモータ15a,15bの速度
を設定する。[0138] In step S318, the counter value is cleared, and in step S319, the maximum inclination angle φmax is cleared. In step S320, the set inclination angle φ
The oscillating speed, that is, the speed of the motors 15a and 15b, corresponding to this is set.
【0139】一方、基準点を見失ったと判断された場合
は、ステップS310からステップS321に進み、前
回算出された傾斜角度を予定角度幅Δφだけ大きくした
角度を今回の設定傾斜角度φとする。On the other hand, if it is determined that the reference point has been lost, the process proceeds from step S310 to step S321, and the angle obtained by increasing the previously calculated inclination angle by the planned angle width Δφ is set as the currently set inclination angle φ.
【0140】こうして、このテーブル傾斜角度設定処理
では、この処理を20回行う間の最大傾斜角度φmax
を設定傾斜角度φとするようにした。つまり、この処理
を20回行う毎に傾斜角度φを更新するようにした。[0140] Thus, in this table inclination angle setting process, the maximum inclination angle φmax during this process is performed 20 times.
is set as the set inclination angle φ. In other words, the inclination angle φ is updated every time this process is performed 20 times.
【0141】なお、この処理による設定傾斜角度φの更
新は当該処理を20回行う毎に限らず、回数は自走車1
の走行路面の状態などによって任意に設定できる。また
、基準点を見失った場合の処理(ステップS321)と
、そうでない場合(ステップS316)とで角度幅Δφ
は異なる値にしてもよい。[0141] Note that the set inclination angle φ is updated by this process not only every 20 times, but also every 20 times.
It can be set arbitrarily depending on the condition of the road surface. In addition, the angular width Δφ is determined by the process when the reference point is lost (step S321) and when it is not (step S316).
may have different values.
【0142】次に、光跡の垂直方向の最大間隔Hと回転
中心軸8の傾斜角度φ、ならびに揺動速度の関係に基づ
いて、反射器6を検出可能な傾斜角度φと揺動速度との
関係がどのようになるかを説明する。図25には、傾斜
角度φが7.5°、5°、および2.5°のときの光跡
の最大間隔Hと揺動速度(回転中心軸8の旋回回数)と
の関係を示した。なお、回転走査速度つまりミラー4の
回転速度は2700rpm、光跡の最大間隔Hは走査装
置から100mの距離における値を示す。同図から分か
るように、光反射器の反射面の高さが1.5mのときに
は検出可能ゾーンはハッチングで示した領域になる。[0142] Next, based on the relationship between the maximum distance H in the vertical direction of the light traces, the inclination angle φ of the rotation center axis 8, and the swinging speed, the inclination angle φ and swinging speed at which the reflector 6 can be detected are determined. Explain how the relationship between FIG. 25 shows the relationship between the maximum interval H of light traces and the swing speed (the number of turns of the rotation center shaft 8) when the inclination angle φ is 7.5°, 5°, and 2.5°. . Note that the rotational scanning speed, that is, the rotational speed of the mirror 4, is 2700 rpm, and the maximum distance H between light traces is a value at a distance of 100 m from the scanning device. As can be seen from the figure, when the height of the reflective surface of the light reflector is 1.5 m, the detectable zone is the area shown by hatching.
【0143】図25に示した関係から、高さ1.5mの
反射面を有する光反射器に光ビームを照射できるための
傾斜角度φと揺動速度との関係は図26のようになる。
同図に示したように、自走車1の走行路面の状態が劣悪
で、傾斜角度を7.5°まで大きく設定しなければなら
ないときは、揺動速度は45rpmまで低下させる必要
がある。これに対し、走行路面が良好であって傾斜角度
が2.5°でも反射器を検出できる場合は、揺動速度は
、90rpmまで増大させられる。単位揺動サイクル毎
に位置検出して操向制御を行うような場合は、このよう
に揺動速度を大きくすることによって自走車1の誘導精
度を向上させることができる。From the relationship shown in FIG. 25, the relationship between the tilt angle φ and the swinging speed for irradiating a light beam onto a light reflector having a reflection surface with a height of 1.5 m is as shown in FIG. As shown in the figure, when the condition of the road surface on which the self-propelled vehicle 1 is traveling is poor and the inclination angle must be set as large as 7.5 degrees, the rocking speed needs to be reduced to 45 rpm. On the other hand, if the road surface is good and the reflector can be detected even with an inclination angle of 2.5 degrees, the rocking speed is increased to 90 rpm. If the steering control is performed by detecting the position every unit swing cycle, the guidance accuracy of the self-propelled vehicle 1 can be improved by increasing the swing speed in this manner.
【0144】前記イニシャルテーブル傾斜角設定処理お
よびテーブル傾斜角設定処理で算出された設定傾斜角度
φに対応する揺動速度を得るためには、図26に示した
テーブルをROMにあらかじめ記憶しておき、傾斜角度
φをアドレスとしてこれに基づいて揺動速度を呼出せば
よい。In order to obtain the rocking speed corresponding to the set tilt angle φ calculated in the initial table tilt angle setting process and the table tilt angle setting process, the table shown in FIG. 26 must be stored in the ROM in advance. , the swing speed can be called based on the inclination angle φ as an address.
【0145】次に、以上の動作を行わせるための制御機
能を説明する。まず、反射光受光処理の機能について説
明する。図27は反射光受光処理部の要部機能を示すブ
ロック図である。同図に示した反射光受光処理部の機能
は、図8,図11,図12のフローチャートに示した反
射光受光処理、イニシャルポール識別処理、ポール選択
処理の内容に対応する。Next, the control function for performing the above operations will be explained. First, the function of reflected light reception processing will be explained. FIG. 27 is a block diagram showing the main functions of the reflected light reception processing section. The functions of the reflected light reception processing section shown in the figure correspond to the contents of the reflected light reception processing, initial pole identification processing, and pole selection processing shown in the flowcharts of FIGS. 8, 11, and 12.
【0146】同図において、外部からの光信号は方位角
検出部37および揺動方向検出部38に入力される。方
位角検出部37はエンコーダ7から入力されるパルス信
号の数を計数するカウンタを持っており、光信号が入力
された時のパルス計数値に基づき、自走車1から見た光
信号入射方位(=方位角)が検出される。検出された方
位角はブロック別方位角記憶部39に記憶される。In the figure, an external optical signal is input to an azimuth angle detection section 37 and a swing direction detection section 38. The azimuth detection unit 37 has a counter that counts the number of pulse signals input from the encoder 7, and determines the optical signal incident azimuth as seen from the self-propelled vehicle 1 based on the pulse count value when the optical signal is input. (=azimuth angle) is detected. The detected azimuth is stored in the block-specific azimuth storage section 39.
【0147】例えば、最初に検出された方位角は第1検
出ブロックの方位角として記憶領域Am(0)に格納さ
れる。第2回目に検出された方位角は、これが第1検出
ブロックで先に検出された方位角とほぼ一致していれば
、Am(0)の記憶データは第2回目に検出された方位
角で更新され、一致していなければ第2検出ブロックの
方位角として新たに記憶領域Am(1)に格納される。
こうして同一方向から入射した光信号は同一の検出ブロ
ックのデータAm(i)として記憶される。For example, the first detected azimuth is stored in the storage area Am(0) as the azimuth of the first detection block. If the azimuth detected the second time almost matches the azimuth detected earlier in the first detection block, the stored data of Am(0) will be the azimuth detected the second time. It is updated, and if it does not match, it is newly stored in the storage area Am(1) as the azimuth of the second detection block. In this way, optical signals incident from the same direction are stored as data Am(i) of the same detection block.
【0148】ブロック別受光回数記憶部40には、前記
検出ブロック別に受光回数が記憶される。受光回数判定
部41は、受光回数が多い検出ブロックを判別し、受光
回数が多い順に4つの検出ブロックを抽出する。そして
、それらの検出ブロックを代表する方位角を、前記ブロ
ック別方位角記憶部39から読出して方位角記憶部42
に格納する。この際、方位角の小さい順に記憶領域Ap
s(1)〜Aps(4)に格納する。予測方位角記憶部
43には、今回検出された方位角に基づいて次回の走査
で検出されるはずの方位つまり予測方位角を記憶する。
この予測方位角は今回検出の方位角と同値でも良いし、
予定の値(予測変化量α)を加算した値でも良いことは
前に述べたとおりである。[0148] The block-by-block light reception count storage section 40 stores the light reception count for each detection block. The light reception frequency determination unit 41 determines which detection block has received the most number of light receptions, and extracts four detection blocks in descending order of the number of light receptions. Then, the azimuths representing those detection blocks are read out from the block-by-block azimuth storage section 39 and stored in the azimuth storage section 42.
Store in. At this time, storage areas Ap
Stored in s(1) to Aps(4). The predicted azimuth storage unit 43 stores the azimuth that should be detected in the next scan based on the azimuth detected this time, that is, the predicted azimuth. This predicted azimuth may be the same value as the currently detected azimuth,
As mentioned above, a value obtained by adding the planned value (predicted change amount α) may be used.
【0149】方位角比較部44では、予測方位角と今回
の検出方位角とが比較され、双方の角度がほぼ一致した
場合は、一致信号aを出力する。この一致信号に応答し
て、常時開のスイッチSW1およびSW2が閉成されて
、基準点別データ記憶部45に各種データが入力されて
記憶される。この各種データは、方位角Ap,揺動方向
As,およびセンサ33の出力を基準とし、その出力時
点からのミラー4の回転回数Cm、ならびに基準点別受
光回数Cp(n)である。ここで、値Ap,As,Cm
は、基準点nおよび基準点別受光回数Cp(n)の関数
として記憶される。The azimuth comparison section 44 compares the predicted azimuth and the currently detected azimuth, and outputs a coincidence signal a if the two angles almost match. In response to this coincidence signal, the normally open switches SW1 and SW2 are closed, and various data are input to and stored in the data storage section 45 for each reference point. These various data are based on the azimuth Ap, the swing direction As, and the output of the sensor 33, and are the number of rotations Cm of the mirror 4 from the time of output, and the number of light receptions Cp(n) for each reference point. Here, the values Ap, As, Cm
is stored as a function of the reference point n and the number of light receptions for each reference point Cp(n).
【0150】次に、ポール(基準点)位置計測処理部の
機能について説明する。図28はポール位置計測処理部
の要部機能を示すブロック図である。同図において、連
続受光検出部46は、反射光受光時の前記ミラー回転数
Cmに基づいて、ミラー4の1回転毎に光信号が連続し
て検出されているかどうかを判断する。そして、最も多
くの光信号を連続受光している場合の、連続期間におけ
る揺動方向のすべての値を、前記基準点別データ記憶部
45から揺動方向範囲演算部47に読み出す。揺動方向
範囲演算部47では、供給された揺動方向データの最大
値および最小値に基づいて揺動方向の範囲が演算される
。Next, the function of the pole (reference point) position measurement processing section will be explained. FIG. 28 is a block diagram showing the main functions of the pole position measurement processing section. In the figure, the continuous light reception detection section 46 determines whether the optical signal is continuously detected every rotation of the mirror 4, based on the mirror rotation number Cm when the reflected light is received. Then, all the values of the swing direction in the continuous period when the largest number of optical signals are continuously received are read out from the reference point data storage section 45 to the swing direction range calculation section 47. The swing direction range calculating section 47 calculates the range of the swing direction based on the maximum value and minimum value of the supplied swing direction data.
【0151】基準点別捕捉揺動方向演算部48では、揺
動方向の範囲を示すデータに基づき、この範囲の中点を
算出する。この中点の値Asc(n)はミラー方向固定
部49に供給される。一方、前記方位角記憶部42から
ミラー固定部49に方位角θ(n)が供給される。ミラ
ー方向固定部49では、これらのデータθ(n)とAs
c(n)を目標値としてミラー4の方向、つまり揺動方
向と方位とを調整してこれらの目標値で示される方向に
ミラー4を固定する。[0151] The reference point-by-reference point acquisition swing direction calculating section 48 calculates the midpoint of the range of swing directions based on data indicating the range of swing directions. This midpoint value Asc(n) is supplied to the mirror direction fixing section 49. On the other hand, the azimuth angle θ(n) is supplied from the azimuth storage section 42 to the mirror fixing section 49. In the mirror direction fixing section 49, these data θ(n) and As
Using c(n) as a target value, the direction of the mirror 4, that is, the swing direction and azimuth, are adjusted to fix the mirror 4 in the direction indicated by these target values.
【0152】ミラー4が固定されると、距離測定部50
で自走車1および各基準点間の距離が測定される。距離
の測定は、例えば、発光部51から発射された光信号の
位相と、受光部52で検出された光信号の位相との差に
基づいて演算される。基準点位置演算部53では、測定
された距離と方位角θ(n)とに基づいて基準点の位置
座標[X(n),Y(n)]が算出される。[0152] When the mirror 4 is fixed, the distance measuring section 50
The distance between the self-propelled vehicle 1 and each reference point is measured. The distance measurement is calculated, for example, based on the difference between the phase of the optical signal emitted from the light emitting section 51 and the phase of the optical signal detected by the light receiving section 52. The reference point position calculation unit 53 calculates the position coordinates [X(n), Y(n)] of the reference point based on the measured distance and azimuth θ(n).
【0153】続いて、自走車1走行中の制御を行う操向
制御部の機能を説明する。図29は操向制御部の要部機
能を示すブロック図である。同図において、前記基準点
位置演算部53で算出された基準点の位置を示す座標は
、座標変換部54で座標変換された後、自走車1の位置
および進行方向を算出する位置・進行方向演算部55に
供給される。すなわち、基準点位置演算部53で算出さ
れた座標は自走車1を原点とする座標系における位置座
標であるため、これを、基準点A〜Dのいずれか1つを
原点とする座標系上での位置に変換するのである。[0153] Next, the function of the steering control section that controls the self-propelled vehicle 1 while it is running will be explained. FIG. 29 is a block diagram showing the main functions of the steering control section. In the figure, the coordinates indicating the position of the reference point calculated by the reference point position calculating section 53 are converted into coordinates by the coordinate conversion section 54, and then the coordinates are used to calculate the position and traveling direction of the self-propelled vehicle 1. The signal is supplied to the direction calculation section 55. That is, since the coordinates calculated by the reference point position calculation unit 53 are position coordinates in the coordinate system with the self-propelled vehicle 1 as the origin, this is the position coordinate in the coordinate system with the origin at any one of the reference points A to D. It converts to the position above.
【0154】位置・進行方向演算部55では、基準点A
〜Dの位置座標および反射光受光処理によって検出され
た方位角θ(n)に基づき、自走車1の位置(Xp,Y
p)、および進行方向θfが算出される。[0154] In the position/progressing direction calculation section 55, the reference point A
The position of the self-propelled vehicle 1 (Xp, Y
p) and the traveling direction θf are calculated.
【0155】また、ターン解除角設定部56では、基準
点の座標およびあらかじめ決められている旋回終了位置
の座標に基づいて右ターン解除角、左ターン解除角およ
び最終ターン解除角がそれぞれ計算されて設定される。
比較部57では、自走車1の位置および進行方向と走行
コース設定部58で設定された走行コースとを比較して
その偏差を操舵部59に出力する。操舵部59は供給さ
れた偏差に基づいてこれを補正するための操舵角を決定
する。決定された操舵角は操舵モータ60に供給され、
この操舵角に従って車輪が操舵される。Further, the turn release angle setting section 56 calculates the right turn release angle, the left turn release angle, and the final turn release angle, respectively, based on the coordinates of the reference point and the predetermined coordinates of the turn end position. Set. The comparing section 57 compares the position and traveling direction of the self-propelled vehicle 1 with the traveling course set by the traveling course setting section 58 and outputs the deviation to the steering section 59. The steering unit 59 determines a steering angle for correcting the supplied deviation based on the supplied deviation. The determined steering angle is supplied to the steering motor 60,
The wheels are steered according to this steering angle.
【0156】さらに、比較部57は自走車1の位置およ
び進行方向と走行コースとを比較して自走車1を旋回さ
せるタイミングを検出する。そして、このタイミングを
検出したならば、検出信号を操舵角固定部61に出力す
る。操舵角固定部61はこの検出信号に応答して予定の
ターン用操舵角を操舵部59に供給し、操舵角を固定さ
せる。Furthermore, the comparing section 57 compares the position and traveling direction of the self-propelled vehicle 1 with the traveling course to detect the timing for turning the self-propelled vehicle 1. When this timing is detected, a detection signal is output to the steering angle fixing section 61. In response to this detection signal, the steering angle fixing section 61 supplies the planned turn steering angle to the steering section 59, thereby fixing the steering angle.
【0157】解除検知部62は、自走車1の進行方向が
ターン解除角に一致するのを監視している。そして、両
者が一致したならば、操舵角固定部61にターン用操舵
角の出力を停止させる指令信号を出力して旋回を終了さ
せる。[0157] The release detection section 62 monitors whether the traveling direction of the self-propelled vehicle 1 matches the turn release angle. If the two match, a command signal is output to the steering angle fixing section 61 to stop outputting the steering angle for the turn, and the turning is completed.
【0158】このように、本実施例では、比較部57で
得られた偏差によって決定される操舵角およびターン用
の固定操舵角に従って車輪を操舵し、予定の走行コース
に自走車1を走行させるようにしている。As described above, in this embodiment, the wheels are steered according to the steering angle determined by the deviation obtained by the comparing section 57 and the fixed steering angle for turning, and the self-propelled vehicle 1 is driven along the planned travel course. I try to let them do it.
【0159】次に、図30に示した機能ブロック図を参
照してイニシャルテーブル傾斜角設定処理部の機能を説
明する。同図において、最離隔基準点判定部63では、
前記基準点位置演算部53で算出された基準点の位置に
基づいて自走車1から最も遠い基準点を識別する。ここ
で検出された最も遠い基準点の方位角が前記方位角記憶
部42から読出され、設定傾斜角度演算部64に供給さ
れる。設定傾斜角度演算部64では、方位角θ(n)お
よび初期設定傾斜角度φTに基づき、前記算出式(2)
,(3)を使用して設定傾斜角度φが計算される。Next, the functions of the initial table inclination angle setting processing section will be explained with reference to the functional block diagram shown in FIG. In the figure, the furthest reference point determination unit 63:
Based on the position of the reference point calculated by the reference point position calculating section 53, the farthest reference point from the self-propelled vehicle 1 is identified. The azimuth angle of the farthest reference point detected here is read out from the azimuth storage section 42 and supplied to the set inclination angle calculation section 64 . The set inclination angle calculation unit 64 calculates the calculation formula (2) based on the azimuth angle θ(n) and the initial set inclination angle φT.
, (3) is used to calculate the set inclination angle φ.
【0160】算出された傾斜角度φは、モータ回転角演
算部65および揺動速度設定部66に供給される。モー
タ回転角演算部65では、算出された前記傾斜角度φで
前記回転中心軸8を揺動させるために必要な、モータ1
5a,15bの回転角が決定される。また、揺動速度設
定部66では、傾斜角度φに応じた揺動速度を得るため
のモータ15a,15bの回転速度が決定される。この
揺動速度設定部66には、例えば図26に示したテーブ
ルを記憶しておき、設定傾斜角度演算部64から供給さ
れた傾斜角度φに応答し、モータ制御部67に対して所
定の揺動速度を示すデータを出力するようする。モータ
制御部67は、供給されたデータに従って、モータ15
a,15bにパルス信号を出力してモータを回動させる
。The calculated inclination angle φ is supplied to the motor rotation angle calculation section 65 and the swing speed setting section 66. The motor rotation angle calculation unit 65 calculates the motor 1 necessary for swinging the rotation center shaft 8 at the calculated inclination angle φ.
The rotation angles of 5a and 15b are determined. Further, the swing speed setting section 66 determines the rotational speed of the motors 15a and 15b to obtain a swing speed corresponding to the inclination angle φ. The swing speed setting unit 66 stores a table shown in FIG. 26, for example, and controls the motor control unit 67 to perform a predetermined swing in response to the tilt angle φ supplied from the set tilt angle calculation unit 64. Output data indicating the moving speed. The motor control unit 67 controls the motor 15 according to the supplied data.
Pulse signals are output to a and 15b to rotate the motor.
【0161】次に、図31を参照してテーブル傾斜角設
定処理の機能を説明する。同図において図30と同符号
は同一または同等部分を示す。同図において、受光判定
部68では、基準点からの反射光を検出したか否か、つ
まり基準点に設置された反射器に光ビームを照射できる
範囲で揺動走査されているかどうかを判定する。反射光
が検出されていれば、傾斜角度演算部69で、前記予定
角度Δφを付加しない傾斜角度φxを算出する。この傾
斜角度φxは最大傾斜角度判定部70において、所定の
処理回数内における最大の傾斜角度φmaxとの大小比
較がなされる。今回算出された傾斜角度φxが、前記最
大傾斜角度φmaxより大きい場合は、最大傾斜角度記
憶部71に記憶されている最大傾斜角度φmaxは今回
の計算値で更新される。Next, the function of table inclination angle setting processing will be explained with reference to FIG. In the figure, the same reference numerals as in FIG. 30 indicate the same or equivalent parts. In the figure, the light reception determination unit 68 determines whether reflected light from the reference point is detected, that is, whether swing scanning is performed within a range that allows the light beam to irradiate the reflector installed at the reference point. . If the reflected light is detected, the inclination angle calculation unit 69 calculates the inclination angle φx without adding the planned angle Δφ. This inclination angle φx is compared in magnitude with the maximum inclination angle φmax within a predetermined number of processing times in the maximum inclination angle determination section 70. If the currently calculated inclination angle φx is larger than the maximum inclination angle φmax, the maximum inclination angle φmax stored in the maximum inclination angle storage section 71 is updated with the currently calculated value.
【0162】カウンタ72は、この最大傾斜角度の判定
が行われた回数が計数される。カウンタ72が予定の回
数を計数したときにカウントアップ信号が出力され、こ
れに応答して最大傾斜角度記憶部71から、その時点の
記憶内容が読出される。この記憶内容つまり最大傾斜角
度φmaxは設定傾斜角度演算部64に供給され、この
最大傾斜角度φmaxに角度Δφが付加される。設定傾
斜角度演算部64で算出された傾斜角度φは設定角度記
憶部73に記憶されると共に、モータ回転角演算部65
および揺動速度設定部66に供給される。[0162] The counter 72 counts the number of times the maximum inclination angle has been determined. When the counter 72 counts the scheduled number of times, a count-up signal is output, and in response to this, the stored contents at that time are read from the maximum inclination angle storage section 71. This stored content, that is, the maximum inclination angle φmax, is supplied to the set inclination angle calculating section 64, and an angle Δφ is added to this maximum inclination angle φmax. The tilt angle φ calculated by the set tilt angle calculation unit 64 is stored in the set angle storage unit 73, and is also stored in the motor rotation angle calculation unit 65.
and is supplied to the swing speed setting section 66.
【0163】一方、受光判定部68において、反射光が
検出されなかったと判定された場合、設定傾斜角記憶部
64では、設定角度記憶部73に記憶されている設定角
度に予定角度Δφを付加し、その値を今回の設定傾斜角
度とする。なお、カウンタ72の値はカウントアップ後
にクリアされ、最大傾斜角度記憶部71に記憶されてい
る最大傾斜角度φmaxもクリアされる。On the other hand, if the light reception determining unit 68 determines that no reflected light is detected, the set tilt angle storage unit 64 adds the planned angle Δφ to the set angle stored in the set angle storage unit 73. , that value is the currently set inclination angle. Note that the value of the counter 72 is cleared after counting up, and the maximum inclination angle φmax stored in the maximum inclination angle storage section 71 is also cleared.
【0164】なお、本実施例では、基準点を見失ったた
めに傾斜角度φを大きくしたとき、つまり揺動走査にお
ける揺動幅BBを大きくしたときには揺動速度を低下さ
せるようにした。したがって、揺動速度に同期させて自
走車1の位置検出を行い、その結果に基づいて移動体の
操向制御を行うようなシステムでは、移動体が走行する
路面の状態が悪くて基準点を頻繁に見失うことがあると
、移動体の誘導精度が悪くなることがある。このような
、路面が劣悪な場合に対処するためには、揺動速度の低
減に伴って移動体の走行速度も落とすことによって誘導
精度が落ちることを回避できる。In this embodiment, when the inclination angle φ is increased because the reference point is lost, that is, when the swing width BB in the swing scan is increased, the swing speed is reduced. Therefore, in a system that detects the position of the self-propelled vehicle 1 in synchronization with the rocking speed and controls the steering of the mobile body based on the results, it is difficult to detect the position of the self-propelled vehicle 1 when the reference point is If you frequently lose sight of the moving object, the accuracy of guiding the moving object may deteriorate. In order to deal with such a case where the road surface is poor, the traveling speed of the moving object is also reduced in conjunction with the reduction in the swinging speed, thereby avoiding a drop in guidance accuracy.
【0165】[0165]
【発明の効果】以上の説明から明らかなように、本発明
によれば、移動体から投射する光ビームの回転走査の光
跡が、移動体から離れた位置に立設されている光反射手
段に対して必ずこれを横切るようにして照射できるよう
に設定可能になるため、移動体自体が傾斜しつづけたり
揺れたりした場合にも、基準点に配置された光反射手段
に対し、光ビームを照射しやすくなる。つまり、基準点
を見失うことがなくなり、光反射手段からの反射光検出
信号に基づく移動体の位置検出精度が高くなって誘導の
精度が格段に向上する。As is clear from the above description, according to the present invention, the light trace of the rotational scanning of the light beam projected from the moving object is reflected by the light reflecting means erected at a position away from the moving object. The light beam can be set so that it always crosses the light beam, so even if the moving object itself continues to tilt or shake, the light beam can be set to always cross the light beam to the light reflecting means placed at the reference point. It becomes easier to irradiate. In other words, the reference point will not be lost, the position detection accuracy of the moving body based on the reflected light detection signal from the light reflecting means will be increased, and the guidance accuracy will be significantly improved.
【0166】また、移動体から最も遠く離れた基準点に
合わせて、この基準点に立設された光反射手段に対して
確実に光ビームを照射できる最小の揺動幅を設定できる
。そしてその揺動幅は小さくなるほど揺動速度を増大さ
せるようにしたので、揺動走査に同期させて移動体の位
置検出を行うようにしたとき、高い頻度で位置検出を行
える。その結果、この位置検出結果に基づいて行われる
移動体の操向制御において移動体の誘導精度を向上する
ことができる。Furthermore, in accordance with the reference point that is farthest from the moving object, the minimum swing width that can reliably irradiate the light beam to the light reflecting means erected at this reference point can be set. Since the swing speed is increased as the swing width becomes smaller, when the position of the moving body is detected in synchronization with the swing scan, the position can be detected with high frequency. As a result, it is possible to improve the guiding accuracy of the moving object in the steering control of the moving object performed based on the position detection result.
【0167】さらに、基準点に立設された光反射手段を
見失ったときには、揺動幅を増大できる。そしてこの場
合には揺動速度を低減できるので、光跡の最大間隔は、
光反射手段より大きくならないように光反射手段の高さ
に応じて最大値を制限することができる。したがって、
瞬間的に基準点を見失うことがあっても、その見失い状
態が長く続くことを防止できる。Furthermore, when the light reflecting means erected at the reference point is lost, the swing width can be increased. In this case, the swing speed can be reduced, so the maximum interval between light trails is
The maximum value can be limited depending on the height of the light reflecting means so that it does not become larger than the light reflecting means. therefore,
Even if you momentarily lose sight of the reference point, this state of loss can be prevented from continuing for a long time.
【図1】 光ビーム走査装置の要部断面図である
。FIG. 1 is a sectional view of a main part of a light beam scanning device.
【図2】 自走車の走行状態を示す斜視図である
。FIG. 2 is a perspective view showing a running state of the self-propelled vehicle.
【図3】 光ビームの光跡を示す斜視図である。FIG. 3 is a perspective view showing a light trail of a light beam.
【図4】 光跡と光反射器との関係を示す図であ
る。FIG. 4 is a diagram showing the relationship between light trails and light reflectors.
【図5】 自走車位置算出の原理説明図である。FIG. 5 is a diagram explaining the principle of self-propelled vehicle position calculation.
【図6】 自走車進行方向算出の原理説明図であ
る。FIG. 6 is a diagram explaining the principle of calculating the traveling direction of a self-propelled vehicle.
【図7】 自走車の走行コースと反射器の配置状
態を示す図である。FIG. 7 is a diagram showing the driving course of a self-propelled vehicle and the arrangement of reflectors.
【図8】 反射光受光処理のフローチャートであ
る。FIG. 8 is a flowchart of reflected light reception processing.
【図9】 自走車の操向制御を示すフローチャー
トである。FIG. 9 is a flowchart showing steering control of a self-propelled vehicle.
【図10】 自走車の操向制御を示すフローチャート
である。FIG. 10 is a flowchart showing steering control of a self-propelled vehicle.
【図11】 イニシャルポール識別処理のフローチャ
ートである。FIG. 11 is a flowchart of initial pole identification processing.
【図12】 ポール選択処理のフローチャートである
。FIG. 12 is a flowchart of pole selection processing.
【図13】 ポール捕捉揺動方向決定処理のフローチ
ャートである。FIG. 13 is a flowchart of pole capture swing direction determination processing.
【図14】 ポール捕捉揺動方向決定処理のフローチ
ャートである。FIG. 14 is a flowchart of pole capture swing direction determination processing.
【図15】 ポール位置計測処理のフローチャートで
ある。FIG. 15 is a flowchart of pole position measurement processing.
【図16】 自走車の往路直進処理のフローチャート
である。FIG. 16 is a flowchart of straight forward processing for the self-propelled vehicle.
【図17】 右ターン解除角セット処理のフローチャ
ートである。FIG. 17 is a flowchart of right turn release angle setting processing.
【図18】 右ターン解除角算出の原理図である。FIG. 18 is a diagram showing the principle of right turn release angle calculation.
【図19】 Uターン処理のフローチャートである。FIG. 19 is a flowchart of U-turn processing.
【図20】 イニシャルテーブル傾斜角度設定処理の
フローチャートである。FIG. 20 is a flowchart of initial table inclination angle setting processing.
【図21】 テーブル傾斜角度設定処理のフローチャ
ートである。FIG. 21 is a flowchart of table inclination angle setting processing.
【図22】 傾斜角度算出式の説明図である。FIG. 22 is an explanatory diagram of a tilt angle calculation formula.
【図23】 検出ブロック別の方位角と受光回数のデ
ータ例を示す図である。FIG. 23 is a diagram showing an example of data of the azimuth angle and the number of times of light reception for each detection block.
【図24】 基準点を検出したときのミラー回転数と
揺動方向のデータを示す図である。FIG. 24 is a diagram showing data on the mirror rotation speed and rocking direction when the reference point is detected.
【図25】 光跡の最大間隔と揺動速度との関係図で
ある。FIG. 25 is a diagram showing the relationship between the maximum distance between light traces and the swing speed.
【図26】 回転中心軸の傾斜角度と揺動速度との関
係図である。FIG. 26 is a diagram showing the relationship between the inclination angle of the rotation center axis and the swing speed.
【図27】 反射光受光処理部の要部機能を示すブロ
ック図である。FIG. 27 is a block diagram showing main functions of a reflected light reception processing section.
【図28】 基準点検出処理部の要部機能を示すブロ
ック図である。FIG. 28 is a block diagram showing main functions of a reference point detection processing section.
【図29】 自走車操向制御部の要部機能を示すブロ
ック図である。FIG. 29 is a block diagram showing main functions of a self-propelled vehicle steering control section.
【図30】 イニシャルテーブル傾斜角度設定処理部
の要部機能を示すブロック図である。FIG. 30 is a block diagram showing main functions of an initial table inclination angle setting processing section.
【図31】 テーブル傾斜角度設定処理部の要部機能
を示すブロック図である。FIG. 31 is a block diagram showing main functions of a table inclination angle setting processing section.
【図32】 従来装置による光ビームの光跡を示す図
である。FIG. 32 is a diagram showing a light trail of a light beam by a conventional device.
Claims (4)
から投射された光ビームを、前記移動体を中心として円
周方向に回転走査する回転走査手段と、前記移動体から
離れた位置に設置された光反射手段で反射された前記光
ビームを前記移動体に搭載された受光手段で受光して前
記光反射手段の方位角を検出する手段とを有し、検出さ
れた前記方位角および基準点の位置情報に基づいて前記
移動体の位置を検出する移動体の位置検出装置において
、前記光ビームを上下方向にも揺動させるための揺動走
査手段を有し、この上下方向の揺動の1サイクル中に光
ビームの回転走査が複数回行われるように構成すると共
に、前記受光手段で検出された反射光の方位角を記憶す
る手段と、前記反射光が検出されていることを条件に前
記揺動走査幅を小さく設定する揺動幅設定手段と、前記
揺動走査幅に応じ、この揺動走査幅が小さく設定された
ときは高い揺動走査速度を設定し、大きく設定されたと
きは低い揺動走査速度を設定する揺動速度設定手段とを
具備し、前記記憶された方位角に基づいて前記揺動走査
に同期して前記移動体の位置を算出するように構成した
ことを特徴とする移動体の位置検出装置。1. Rotary scanning means for rotating and scanning a light beam projected from a light beam generating means mounted on a moving body in a circumferential direction centering on the moving body, and a rotary scanning means installed at a position distant from the moving body. means for detecting the azimuth angle of the light reflecting means by receiving the light beam reflected by the light reflecting means mounted on the movable body, and detecting the azimuth angle of the light reflecting means, the detected azimuth angle and a reference. A position detecting device for a moving body that detects the position of the moving body based on positional information of a point, the apparatus having a swing scanning means for swinging the light beam in the vertical direction as well; The light beam is configured so that rotational scanning is performed a plurality of times during one cycle of the light beam, and means for storing the azimuth angle of the reflected light detected by the light receiving means, and a condition that the reflected light is detected. an oscillation width setting means for setting the oscillation scan width to a small value; and a swing speed setting means for setting a low swing scan speed, and the position of the moving object is calculated in synchronization with the swing scan based on the stored azimuth angle. A moving body position detection device characterized by:
揺動走査幅を予定値だけ増大させる手段を具備したこと
を特徴とする請求項1記載の移動体の位置検出装置。2. The position detecting device for a moving body according to claim 1, further comprising means for increasing the swing scanning width by a predetermined value when no reflected light is detected.
この設定値の増減に伴って移動体の移動速度を増減させ
る手段を具備したことを特徴とする請求項1記載の移動
体の位置検出装置。3. According to the set value of the swing scanning speed,
2. A position detecting device for a movable body according to claim 1, further comprising means for increasing or decreasing the moving speed of the movable body as the set value increases or decreases.
段の回転中心軸を、その回動軌跡が略円錐形状となるよ
うに揺動させる手段を具備し、前記光ビームの発生手段
と受光手段とが一体的に揺動されるように、この光ビー
ムの発生手段および受光手段を前記揺動手段に装着した
ことを特徴とする移動体の位置検出装置。4. The oscillating scanning means includes means for oscillating the central axis of rotation of the rotary scanning means so that its rotation locus has a substantially conical shape, and the oscillating scanning means includes a means for oscillating the central axis of rotation of the rotary scanning means so that the rotation locus thereof has a substantially conical shape. 1. A position detecting device for a movable body, characterized in that the light beam generating means and the light receiving means are attached to the swinging means so that the light beam generating means and the light receiving means are swingable together.
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP16233891A JPH04361189A (en) | 1991-06-07 | 1991-06-07 | Moving body position detector |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP16233891A JPH04361189A (en) | 1991-06-07 | 1991-06-07 | Moving body position detector |
Publications (1)
Publication Number | Publication Date |
---|---|
JPH04361189A true JPH04361189A (en) | 1992-12-14 |
Family
ID=15752659
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
JP16233891A Pending JPH04361189A (en) | 1991-06-07 | 1991-06-07 | Moving body position detector |
Country Status (1)
Country | Link |
---|---|
JP (1) | JPH04361189A (en) |
-
1991
- 1991-06-07 JP JP16233891A patent/JPH04361189A/en active Pending
Similar Documents
Publication | Publication Date | Title |
---|---|---|
US5258822A (en) | System for detecting the position of a moving body | |
JP2802560B2 (en) | Mobile steering control device | |
AU755485B2 (en) | Method for determining the position of an automated guided vehicle | |
JPH01316808A (en) | Steering controller for self-traveling vehicle | |
JP2712061B2 (en) | Moving object position detection device | |
JP3001701B2 (en) | Mobile steering control device | |
US5260770A (en) | System for detecting the position of observation spot | |
JPH04361189A (en) | Moving body position detector | |
JP3025574B2 (en) | Position detection device | |
JP2947427B2 (en) | Moving object position detection device | |
JP2968612B2 (en) | Moving object position detection device | |
JP2968611B2 (en) | Position detection device | |
JPH0260124B2 (en) | ||
JP3071952B2 (en) | Position detection device | |
JP3172272B2 (en) | Mobile steering control device | |
JP2908601B2 (en) | Position detection device | |
JPH043209A (en) | Steering controller for free-running car | |
JP2908612B2 (en) | Position detection device | |
CN110618677B (en) | Mobile robot | |
JPH04344905A (en) | Steering controller for traveling object | |
EP0943892B1 (en) | Laser beam emitting apparatus | |
JPH0833770B2 (en) | Self-propelled vehicle steering position detection device | |
JPH0771978A (en) | One's own position recognition system | |
JP2802525B2 (en) | Traveling course setting device for self-propelled vehicles | |
JP2533785B2 (en) | Position detection device for moving body |