JPH0659016A - Position measuring device for moving body - Google Patents

Abstract

PURPOSE:To provide position measuring device for a moving body by which a high-precision measuring result can be obtained without requiring preliminarily measuring of the position of a corner cube and registering it into a circuit. CONSTITUTION:A laser navigator and a control circuit and mounted on a self- traveling robot 1, and in measuring the position of corner cubes CC1, CC2, CC3, CC4 with the laser navigator, an incident angle theta1 of reflected light from each of the corner cubes is detected in a state that the self-traveling robot is placed in a first reference position P1, and at the same time an incident angle alpha2 of reflected light from each of the corner cubes is detected in a state that the self-traveling robot 1 is placed in a second reference position P2 apart from the first reference position P1. A control circuit calculates the position of each of the corner cubes on the basis of an interval (a) between the first reference position and the second reference position and an incident angle theta1, theta2 in each reference position.

Description

Detailed Description of the Invention

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to an apparatus for measuring the current position of various moving bodies such as automatic cleaning robots and automatic lawnmower robots in the course of movement.

[0002]

2. Description of the Related Art Conventionally, as such a device, at least three light reflectors (corner cubes) are provided that rotate and emit a laser beam from a moving body and separate the moving body from the moving body. A laser navigator has been proposed that detects the reflected light of a moving object on a moving object and measures the current position of the moving object based on the position of the light reflector and the opening angle between the light reflectors.
(Japanese Patent Publication No. 2-48069, Japanese Patent Laid-Open No. 1-145517).

The device is a cylindrical laser scanner (15) rotatably supported on the upper part of the device body as shown in FIG.
The bottom of the laser scanner (15) is equipped with a cylindrical body (54).
Is projected downward. A semiconductor laser (4) is installed below the cylindrical body (54), and laser light from the semiconductor laser (4) is shaped into parallel light through a lens (41) and then inside the cylindrical body (54). After passing through the mirror, it is reflected in the horizontal direction by the mirror (42) in the laser scanner (15), and further passes through the cylindrical lens (43) to become a vertical surface beam, which is the corner cube (2).
It is emitted toward.

The corner cube (2) has at least 3
The individual pieces are previously positioned and arranged on the moving plane of the moving body.

Further, a DC motor (5) is installed in the main body of the apparatus, and the rotation of the DC motor (5) passes through a driving pulley (51), a timing belt (52) and a driven pulley (53) to form a cylinder. body
Informed to (54). As a result, the cylindrical body (54) and the laser scanner (15) rotate together and the laser scanner (1
The laser beam emitted from 5) is rotated and scanned in the horizontal plane.

Light reflected from the corner cube (2) is a mirror
Reflected downward at (42), mirror (44), condenser lens (45)
And through the interference filter (46), the photodiode (47)
Is received at. The output of the photodiode (47) is sent to the light receiving circuit (62) and becomes a light receiving signal and is input to the arithmetic processing circuit (61). Also, at the lower end of the cylindrical body (54), an encoder
(6) is attached integrally, and the angle signal and the reference signal output from the encoder (6) are input to the arithmetic processing circuit (61).

The arithmetic processing circuit (61) calculates the position and orientation of the moving body from the principle of triangulation based on the input data,
The result is sent to the host computer (3).

According to the laser navigator (13), the position and orientation of the moving body can be calculated with a high resolution of millimeters and milliradians.

[0009]

However, in the conventional laser navigator, the operator pre-measures the position (coordinates) of each corner cube with a measure or the like, and the result is registered in the arithmetic processing circuit (61). Because you need to
Not only the measurement work is complicated, but also some measurement error is unavoidable, and there is a problem that the measurement error appears as a large error in the calculation result of the position and orientation of the moving body.

An object of the present invention is to provide a position measuring device for a moving body, which does not require a procedure of measuring the position of a corner cube in advance and registering it in a circuit, and which can obtain a measurement result with higher accuracy than before. Is to provide.

[0011]

A position measuring device for a moving body according to the present invention is provided for a moving body of reflected light from each light reflector in a state where the moving body is installed at a _first reference position P ₁ . with detecting the incident angle theta _1, in a state in which the moving body is installed on the second reference position P ₂ spaced from the first reference position, the incident angle theta with respect to the movement of the reflected light from the optical reflector ₂ And a calculation means for calculating the position of each light reflector on the basis of the distance a between the both reference positions and the incident angles θ ₁ and θ ₂ with respect to each light reflector at both reference positions. It is equipped with

[0012]

In measuring the position of the moving body, first, at least three light reflectors are installed at arbitrary positions on the moving plane of the moving body. Then, prior to the position measurement of the moving body, the position of each light reflector is measured by optical measurement. At this time, an optical system used for measuring the position of the moving body can be used as the optical system for optical measurement.

First, the moving body is installed at the first reference position, and in this state, the light beam is rotationally scanned and emitted, and the reflected light from each light reflector is received. As a result, the incident angle θ ₁ of the reflected light with respect to the moving body at the first reference position is detected for each light reflector.

Next, the moving body is installed at the second reference position, and in this state, the light beam is rotationally scanned and emitted, and the reflected light from each light reflector is received. As a result, for each light reflector, the incident angle θ ₂ of the reflected light with respect to the moving body at the second reference position is detected.

Here, the distance a between the first reference position and the second reference position can be set to be sufficiently smaller than the distance between the light reflectors, and a well-known length measuring means, for example, a rotary encoder provided on a moving body or the like. Can measure with high accuracy.

Next, the position of each light reflector is calculated based on the measurement results of the incident angles θ ₁ and θ ₂ . On this occasion,
The position of any one light reflector is defined by the distance a between the two reference positions.
Then, since it is defined by _two incident angles θ ₁ and θ ₂ with respect to the light reflector (see FIG. 4), the XY coordinates of the light reflector are calculated based on these data.

After that, the process shifts to the position measurement of the moving body, the reflected light from each reflector is detected with the rotation scanning of the light beam, and the position (XY coordinate) of each light reflector and the space between the light reflectors are detected. The current position of the moving body is calculated based on the opening angle of the.

[0018]

According to the position measuring device for a mobile body of the present invention, the position of each light reflector is measured with high accuracy by optical measurement prior to the position measurement of the mobile body, and based on the measurement data. Since the position of the moving body is calculated, highly accurate measurement results can be obtained. However, the conventional manual position measurement and registration procedure of the light reflector is not required.

[0019]

DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS An embodiment in which a mobile body position measuring apparatus according to the present invention is mounted on a self-propelled robot will be described in detail below with reference to the drawings.

As shown in FIG. 1, the self-propelled robot (1) is equipped with a working mechanism (11) for cleaning the floor surface, lawn mowing and the like, and is driven along an arbitrary path by motor driven wheels (12). It is self-propelled. A rotary encoder (not shown) is provided coaxially with the wheels (12) to measure the moving distance by contacting the floor surface.

The self-propelled robot (1) has a laser scanner (1
A laser navigator (13) equipped with 5) is installed. The laser navigator (13) rotationally scans and emits a laser beam from the laser scanner (15), detects the reflected light from the corner cube (2), and measures the current position and orientation of the self-propelled robot (1). The optical system has the same structure as the conventional one shown in FIG.

Data measured by the laser navigator (13) is sent to a control circuit (14) composed of a microcomputer as shown in FIG. 1 and used for traveling control of the self-propelled robot (1).

As shown in FIG. 2, at least three corner cubes (2), four in the illustrated example, CC1, CC2, CC3 and CC4 are installed on the moving surface of the self-propelled robot (1).

Prior to measuring the position of the self-propelled robot (1),
The position of each corner cube CC1, CC2, CC3 and CC4 is measured by optical measurement.

First, as shown in FIG. 2, the self-propelled robot (1) is the first
It is installed at the reference position P ₁ , and in this state, the light beam is rotationally scanned and emitted, and the reflected light from each corner cube is detected. Next, the self-propelled robot (1) moves to the second reference position P ₂ , and in this state, similarly, the light beam is rotationally scanned and emitted, and the reflected light from each corner cube is detected. Here, the moving distance a from the first reference position P ₁ to the second reference position P ₂ can be set arbitrarily.

FIG. 4 shows the principle of measuring the coordinates (X, Y) of the corner cube CC at an arbitrary position based on the detection of reflected light at the _two reference positions P ₁ and P ₂ . When the traveling direction of the self-propelled robot is set on the y-axis on the xy plane as shown in the figure, the distance between the first reference position P ₁ and the second reference position P ₂ is a, and the corner cube at the first reference position P ₁ CC
Let θ _{1 be} the incident angle of the reflected light from and θ _{2 be} the incident angle of the reflected light from the corner cube CC at the second reference position P ₂ , then the coordinates (X, Y) of the corner cube CC are represented by the following equations. .

[0027]

## EQU1 ## X = a / (tan (π / 2-θ ₁ ) -tan (π / 2-θ ₂ )) Y = a × tan (π / 2-θ ₁ ) / (tan (π / 2- θ ₁ ) −tan (π
/ 2−θ ₂ ))

Here, the distance a between the reference positions P ₁ and P ₂ can be measured with high accuracy by the rotary encoder. In this case, the measurement data of the interval a is automatically fetched by the control circuit (14). Further, the distance a can be set to be sufficiently smaller than the distance between the corner cubes, so that the measurement can be performed with high accuracy even by manual work using a measure. In this case, the measurement data is input using a keyboard or the like connected to the control circuit (14).

In the control circuit (14) shown in FIG. 1, the arithmetic procedure of the above formula 1 is registered, the moving distance a obtained from the rotary encoder and the incident angle θ ₁ obtained from the laser navigator (13), The coordinates of each corner cube are calculated based on θ ₂ .

The coordinates of the corner cubes CC1, CC2, CC3 and CC4 thus calculated are registered in the memory provided in the control circuit as shown in FIG.

FIG. 3 shows a series of control circuits for advancing the original work of the self-propelled robot while calculating the current position and direction in the moving process of the self-propelled robot after calculating the coordinates of each corner cube as described above. Represents the processing procedure of.

First, the reflected light from each corner cube to the self-propelled robot at the first reference position is detected, the incident angle θ ₁ for each corner cube is measured (S1), and the measurement result is stored (S2). ).

Next, the reflected light from each corner cube to the self-propelled robot at the second reference position is detected, and the incident angle θ ₂ for each corner cube is measured (S
3) The measurement result is stored (S4).

Then, the position of each corner cube is calculated based on the above equation 1, and the result is stored.
(S5).

After that, the current position and direction are calculated at an arbitrary point in the moving process of the self-propelled robot (S6), and the original work of the self-propelled robot is performed based on the result as in the conventional case. A host process for executing is executed (S7). For example, in a floor-cleaning self-propelled robot, the position and orientation of the robot on the travel route, which is preset in the control circuit, is corrected based on the actually measured current location and orientation.

Then, it is judged whether or not the work of the self-propelled robot is completed (S8), and the above steps S6 and S7 are repeated until the work is completed. As a result, for example, in the above floor cleaning robot, the floor can be thoroughly cleaned by the accurate movement of the robot.

According to the above apparatus, the position of each corner cube (2) is measured by the laser navigator (13) prior to the position measurement of the self-propelled robot (1). It is possible to obtain highly accurate measurement data as compared with the measurement by. Therefore, self-propelled robot (1)
The laser navigator (1
High-precision measurement results can be obtained without impairing the resolution of 3).

The above description of the embodiments is for explaining the present invention, and should not be construed as limiting the invention described in the claims or reducing the scope. The configuration of each part of the present invention is not limited to the above-mentioned embodiment, and it goes without saying that various modifications can be made within the technical scope described in the claims.

For example, the interval a between the first reference position and the second reference position is set to a predetermined value, and the traveling of the interval is performed by the self-propelled robot (1).
It is also possible to automate all measurements by incorporating them in the travel plan.

[Brief description of drawings]

FIG. 1 is a schematic configuration diagram showing an example in which the present invention is applied to a self-propelled robot.

FIG. 2 is a plan view showing two reference positions of a self-propelled robot when measuring the position of each corner cube.

FIG. 3 is a flowchart showing a processing procedure for position measurement according to the present invention.

FIG. 4 is a plan view illustrating the principle of position measurement of a corner cube.

FIG. 5 is a chart showing the results of position measurement of each corner cube.

FIG. 6 is a block diagram showing an optical system and a circuit configuration of a conventional laser navigator.

[Explanation of symbols]

(1) Self-propelled robot (13) Laser navigator (14) Control circuit (2) Corner cube P ₁ 1st reference position P ₂ 2nd reference position

Claims (1)

[Claims] 1. A light beam is rotatively scanned and emitted from a moving body, and reflected light from at least three light reflectors separately arranged apart from the moving body is detected on the moving body. In a device that measures the current position of a moving body based on the position of each light reflector and the opening angle between the light reflectors, with the moving body installed at the first reference position, The angle of incidence θ ₁ of the reflected light with respect to the moving body and the moving body of the reflected light from each light reflector in the state where the moving body is installed at the second reference position apart from the first reference position. based an optical detection means for detecting the incident angle theta _2, the first reference position and the spacing a of the second reference position, and the incident angle theta ₁ of each optical reflector at both the reference position, the theta ₂ for each A moving body characterized by comprising a calculation means for calculating the position of the light reflector. Location measurement device.