JPH0781858B2 - Vehicle position and attitude angle measuring device - Google Patents

Description

The present invention relates to a vehicle position and angle-of-fit adjustment device, and more particularly to a vehicle required for driving assistance to a work vehicle operating at a construction site and automation of work. The present invention relates to a vehicle position and conforming angle measuring device for obtaining information such as the current position of the vehicle and topography of the site.

[Prior art]

Conventionally, as a system for measuring and using the three-dimensional position of a vehicle for automating the driving support and work of a work vehicle at a construction site, there is JP-A-59-180422 (travel locus analysis system).

This system samples the traveling speed of the vehicle, the vehicle front-rear inclination angle, and the traveling azimuth at regular intervals, and calculates the current position of the vehicle at regular intervals based on these sampled values.

[Problems to be solved by the invention]

However, in the case of the above-mentioned conventional vehicle position measurement system, the current position of the vehicle is calculated by accumulating the relative movement amount of the vehicle, and therefore the current position of the vehicle is caused by road surface irregularities, wheel slips, or sampling value errors. Accumulated error occurs in
There is a problem that the vehicle position measurement accuracy is poor.

The present invention has been made in view of the above circumstances, and it is possible to measure the three-dimensional position of a vehicle and the posture angle of the vehicle with high accuracy in real time even at a construction site where the work road surface shape frequently changes. It is an object of the present invention to provide a vehicle position and attitude angle measuring device.

[Means for solving problems]

In the present invention, in order to achieve the above-mentioned object, the preset 2
First and second light projecting means which are installed at fixed points and respectively project a rotating laser beam which rotates at a fixed cycle, and a light receiving timing and a light receiving height position of the rotating laser beam which are arranged on the vehicle and are detected. Based on the first, second and third light receiving means and the light receiving timing detected by the first light receiving means, the first and second rotary laser light from the first and second light projecting means at the time of receiving the light Based on the second rotation angle and the light receiving timing detected by the second light receiving means, the third and fourth rotation angles of the rotating laser light from the first and second light projecting means at the time of receiving the light are obtained. The rotation angle calculation means, the installation intervals of the first and second light projecting means, and the first, second rotation angle and third, fourth rotation angle calculated by the rotation angle calculation means The first seat on a certain xyz coordinate system of each light receiving position in the first and second light receiving means First computing means for calculating the position and the second coordinate position, the arrangement intervals of the first, second and third light receiving means, the light receiving height positions respectively detected by the light receiving means and the first light receiving means. A front-back inclination angle of the vehicle based on the first and second coordinate positions calculated by the calculation means,
Second computing means for computing the left-right inclination angle and the azimuth angle;
At least one light receiving height position detected by the first, second and third light receiving means, at least one xy coordinate position calculated by the first calculating means, and the front and rear of the vehicle calculated by the second calculating means Third computing means for calculating the xy coordinate position of the vehicle representative point based on the tilt angle, the left-right tilt angle and the azimuth angle, the z coordinate position of the rotating laser light, the first, second and third light receiving means. And a fourth calculation means for calculating the z coordinate position of the vehicle representative point based on at least one light-receiving height position detected by the vehicle and the front-back inclination angle and the left-right inclination angle of the vehicle calculated by the second calculation means. It is characterized by that.

[Action]

First, the installation intervals of the first and second light projecting means and the first and second rotating laser light when the first light receiving means receives the respective rotating laser light from the first and second light projecting means. Based on the second rotation angle, the first xy coordinate position on the certain xyz coordinate system of the light receiving position of the first light receiving means is obtained by the principle of triangulation. Similarly, the second and third xy coordinate positions of the light receiving positions of the second and third light receiving means are obtained.

Since the vehicle is not always in the horizontal state, the xy coordinate position of the vehicle representative point cannot be immediately obtained from the xy coordinate position.
Therefore, the attitude angle of the vehicle (front-back inclination angle, left-right inclination angle, azimuth angle) is calculated. The front-rear inclination angle, the left-right inclination angle, and the azimuth angle of the vehicle are the arrangement intervals of the first, second, and third light receiving means,
It is determined based on the light receiving height position detected by the light receiving means and the first, second, and third xy coordinate positions.

Then, the xy coordinate position of the vehicle representative point is the first, second, third
The z-coordinate position of the vehicle representative point is obtained based on at least one xy-coordinate position of the xy-coordinate position and the calculated attitude angle of the vehicle, and the z-coordinate position of the vehicle representative point is the z-coordinate position of the rotating laser light, the first, second, and third positions. It is determined based on at least one light receiving height position detected by the light receiving means and the attitude angle of the vehicle (in this case, the azimuth angle is excluded).

〔Example〕

 Hereinafter, the present invention will be described in detail with reference to the accompanying drawings.

First, the principle of the method of measuring the three-dimensional position of a vehicle according to the present invention will be described.

In Fig. 2, the distance between points A and B is L, the x-axis and the line segment AC
Assuming that the angle formed by and is α _a , and the angle formed by the x-axis and the line segment BC is α _b , the xy coordinate of the point C is Can be represented by Therefore, in the present invention, when obtaining the above α _a and α _b , a light projecting device for projecting a rotating laser beam at each of points A and B is installed, and a light receiving device is mounted on the vehicle, and this light receiving device is provided as described above. This is performed by detecting the rotation angle of each rotating laser light when the rotating laser light is received.

As shown in FIG. 3, the rotation angles α _a and α _b are the rotation laser beams emitted from the light projecting devices installed at points A and B with the period of the rotation laser light being T (FIG. 3A). Light is the reference direction (x
The time from reaching axially) until it enters the vehicle light receiving device respectively t _a and t _b (FIG. 3 (b) and (c))
Then, Is calculated as follows.

The xy coordinates calculated by the equation (1) are the coordinates of the laser light receiving point of the vehicle-mounted light receiving device, not the coordinates of the vehicle representative point. Therefore, when converting to the coordinates of the vehicle representative point, the attitude angle of the vehicle (front-back inclination angle, left-right inclination angle, azimuth angle) is obtained.

Next, how to obtain the posture angle will be described.

First, consider the vehicle body 1 and the light receiving devices 2, 3 and 4 as modeled as shown in FIG. Here, point P is x'y'z '
The fourth point when the front-back inclination angle θ, the left-right inclination angle δ, and the azimuth angle of the vehicle are 0, respectively, as the coordinate origin of the coordinates.
FIG. 5 shows the coordinate positions in the x'y'z 'coordinate of the points of the vehicle body shown in the figure. Incidentally, points Q, T and W are laser light receiving points in the light receiving devices 2, 3 and 4, respectively.

Now, when the attitude angle of the vehicle body changes, the above Q point,
'Movement position in the coordinate system (x'x'y'z the point T and W point _{q, y 'q, z'} q), (x 't, y' t, z 't) and (x' _w, y ′ _w , z ′ _w ) is Represented by

Therefore, if the position of point P in the xyz coordinate system with the point A in FIG. 2 as the origin is P (x ₀ , y ₀ , z ₀ ), the points Q, T
Positions of point and W in the xyz coordinate system ((x _q , y _q ,
z _q ), (x _t , y _t , z _t ) and (x _w , y _w , z _w ) are Becomes

Here, if the vertical direction (z-axis) height of the laser light surface is h ₀ , then z _q = z _t = z _w = h ₀ . First, the left-right inclination angle θ is obtained. Number (6)
If z ₀ is eliminated from the equations and the equation (8), then z 1 _q = z ′ _w, that is, h ₁ cos δ cos θ = − (c sin δ−h ₃ cos δ) cos θ ......
(9) By rewriting the formula (9), {(h ₁ −h ₃ ) cosδ + c sinδ} cos θ = 0 (10) Since −π / 2 <θ <π / 2 in the formula (10), cos θ ≠ 0 Next, the front-back inclination angle θ is obtained. When z ₀ is eliminated from the equations (6) and (7), z ′ _q = z ′ _t, that is, h ₁ cos δ cos θ = h ₂ cos δ cos θ-b sin θ from the formulas (3) and (4). (13) Rewriting equation (13), Next, the azimuth angle is obtained. Eliminating x ₀ from the equations (6) and (7), x _q −x _t = x ′ _q −x ′ _t = (h ₁ −h ₂ ) cos δ sin θ cos −h ₁ sin δ sin −b cos θ cos …… ( 16) Moreover, if y ₀ is eliminated from the equations (6) and (7), y _q −y _t = y ′ _q −y ′ _t = (h ₁ −h ₂ ) cos δsin θ sin + h ₁ sin δcos −b cos θsin (17) If θ is eliminated from the above equations (16) and (17), h ₁ sin δ = (y _q −y _t ) cos − (x _q −x _t ) sin ＝ u sin (+ v) (18) However, Then, when the above equation (18) is transformed, Becomes That is, the measured h ₁ , h ₂ , h ₃ and (x _q ,
y _q ), (x _t , y _t ) can be used to obtain the vehicle lateral inclination angle δ, longitudinal inclination angle θ, and azimuth angle using the above equations (12), (15), and (19). .

When the posture angle (θ, δ,) of the vehicle is obtained, the position P (x ₀ , y ₀ , z ₀ ) of the point P can be calculated from the equations (3) and (6) as follows: Becomes

Therefore, the position of the point P can be obtained from the equation (20), and the position of the vehicle representative point (for example, the center of gravity) having a fixed relationship with the point P can also be obtained.

FIG. 1 is a block diagram showing an embodiment of the present invention. In the figure, the ground station side comprises a ground reference station 10, two light projecting devices 11 and 12, a map display 13 and a map data storage device 14. The ground reference station 10 further includes a transceiver 10a,
The projecting device 1 includes a processing device 10b and an oscillator 10c.
Reference numerals 1 and 12 include pulse motors 11a and 12a, rotary heads 11a and 12b, and laser oscillators 11c and 12c.

On the other hand, the vehicle is provided with three light receiving devices 2, 3, 4, a transceiver 5, and other devices for performing various calculations, processes, and controls.

The light projecting devices 11 and 12 are installed at points A and B in the xyz coordinate system with a constant interval L as shown in FIG.
Here, the rotating laser light planes from the light projecting devices 11 and 12 are installed so as to be parallel to the xy plane, and the height of each rotating laser light plane is set to a predetermined height h ₀ .

The light projecting device 11 projects the laser light continuously oscillated from the laser oscillator 11c into the rotary head 11b, and rotates the rotary head 11b by the pulse motor 11a to project the rotary laser light from the rotary head 11b. . The pulse motor 11a rotates in synchronization with a clock pulse applied from the oscillator 10c of the ground reference station 10. It should be noted that the projector 12 is also the projector 11
The rotating laser light is projected in the same manner as. That is, the rotational synchronization and phase of the rotary heads 11b and 12b are matched.

Further, the processing unit 10b of the ground reference station 10 detects a time point at which the laser light from the light projecting units 11 and 12 faces the reference azimuth (x-axis direction) by the clock pulse from the oscillator 10c. A reference direction signal indicating the reference direction is generated.

When the vehicle-mounted transceiver 5 receives the reference azimuth signal, it adds it to the vehicle position calculation device 20. Further, the vehicle position calculation device 20 is applied with signals from the light receiving devices 2, 3 and 4 indicating the light receiving timing of the rotating laser light and signals indicating the light receiving heights h ₁ , h ₂ and h ₃ . If the vehicle side is provided with a means for measuring the same time as the oscillator 10c on the ground station side, it is possible to detect only the time point at which the laser light faces the reference azimuth direction.

The vehicle position calculation device 20 first detects the light receiving positions Q and T of each light receiving device.
The coordinate position (x _q , y _q , z _q ) and (x _t , y _t , z _t ) of
It is calculated based on the equation and the equation (2). Note that z _q = z _t =
h ₀ .

Next, the vehicle horizontal inclination angle δ is obtained by substituting the input light receiving height positions h ₁ and h ₃ into the equation (12). Also,
This inclination angle δ and the received light receiving height positions h ₁ and h ₂ are set to the (15)
The vehicle front-rear tilt angle θ is obtained by substituting it into the equation. Furthermore, the tilt angle δ, the calculated (x _q , y _q ), (x _t ,
y _t ) and the light receiving height position h ₁ are substituted into the equation (19) to obtain the azimuth angle.

Then, the light receiving positions Q (x _q , y _q , z _q ) and T (x _t , y _t ,
The three-dimensional position of the vehicle representative point is obtained based on z _t ) and the attitude angle (θ, δ,) obtained as described above. The position P (x ₀ , y ₀ , z ₀ ) at the point P on the vehicle mounting portion of the light receiving device 2 is
It can be obtained by substituting (x _q , y _q , z _q ) and (θ, δ,) into Eq. (12).

The signal indicating the three-dimensional position of the vehicle representative point thus obtained is added to the map data processing device 21. The map data processing device 21 updates the vehicle running locus, topographical data and the like based on the daily map data from the map data storage device 23 and creates new map data.

For example, as shown in Table 1, the traveling area (working area) of a vehicle is divided into a finite number (n × n) in the x and y directions, and a storage unit having each divided position as an address is provided. When the finite number of positions is reached, the map data of the three-dimensional traveling locus can be stored by writing the z coordinate in the storage unit corresponding to the position.

When the position where the z coordinate has already been written is reached, the latest z coordinate is rewritten. If the vehicle representative point is the ground contact point with the ground, the three-dimensional traveling locus data stored as described above can be regarded as the terrain data.

In addition, the map data processing device 21 includes a map data storage device 23.
Based on the map data stored in the map display unit 22, a vehicle running track or topography is displayed on the map display (22). FIG. 6 shows an example of a map display. The theory and the current position of the vehicle can be displayed on the map.

The on-vehicle transceiver 5 transmits the map data stored in the map data storage device, and the transceiver 10a of the ground reference station 10 receives this and adds it to the processing device 10b. The processing device 10b stores the input map data in the map data storage device 14 and causes the map display 13 to display the traveling locus or topography of the vehicle based on the stored map data.

Scheduled work contents are set in the work command generation device 24, and the arithmetic processing device 25 causes the signal indicating the work contents added from the work command generation device and the data (map data, Based on the vehicle position data and attitude angle data), an engine control signal, a gear shift control signal, a steering control signal and a work implement control signal are formed to generate an engine control device 26, a gear change control device 27, a steering control device 28 and a work implement control. Control the device 29.

Note that the above system can constitute a control system for construction work that remotely controls a vehicle by performing data communication with the ground reference station 10 on vehicle position data and terrain data obtained in real time and a work command.

〔The invention's effect〕

As described above, according to the present invention, the three-dimensional position of a vehicle can be obtained accurately and in real time without accumulating measurement errors even on rough terrain such as a construction site. Further, it is possible to provide highly accurate vehicle position information and topographical information to the operator to support the work.

[Brief description of drawings]

FIG. 1 is a block diagram showing an embodiment of the present invention, FIGS. 2 to 5 are diagrams used for explaining a position measuring method according to the present invention, and FIG. 6 is a map display in FIG. It is a figure which shows the example of a display of. 1 ... Car body, 2,3,4 ... Light receiving device, 5 ... Transceiver, 10
...... Ground reference station, 11,12 ...... Projection device, 13,22 ...... Map display device, 14,23 ...... Map data storage device, 20 ...... Vehicle position calculation device, 21 ...... Map data processing device.

Claims (3)

[Claims] 1. A first and a second light projecting means installed at two fixed points set in advance and projecting a rotating laser beam rotating at a constant cycle, respectively, and a light receiving unit for receiving the rotating laser beam arranged on a vehicle. First, second and third light receiving means for detecting timing and light receiving height position, and the first and second light projecting means at the time of receiving light based on the light receiving timing detected by the first light receiving means Based on the first and second rotation angles of the rotating laser light from the first and second light receiving means and the light receiving timing detected by the second light receiving means. Rotation angle calculation means for obtaining third and fourth rotation angles, installation intervals of the first and second light projecting means, first and second rotation angles calculated by the rotation angle calculation means, and third and third rotation angles. Each light receiving position in the first and second light receiving means based on the rotation angle of 4 A first arithmetic means for calculating a first coordinate position and a second coordinate position on a certain xyz coordinate system, and the arrangement intervals of the first, second and third light receiving means, and the light receiving means. Second computing means for computing the longitudinal inclination angle, lateral inclination angle and azimuth angle of the vehicle based on the detected light receiving height position and the first and second coordinate positions calculated by the first computing means, At least one light receiving height position detected by the first, second and third light receiving means, at least one xy coordinate position calculated by the first calculating means, and the front and rear of the vehicle calculated by the second calculating means Third calculating means for calculating the xy coordinate position of the vehicle representative point based on the tilt angle, the left-right tilt angle and the azimuth angle, the z coordinate position of the rotating laser light, the first, second and third light receiving means At least one of the light receiving height positions detected by And a fourth calculating means for calculating the z-coordinate position of the vehicle representative point based on the longitudinal inclination angle and the lateral inclination angle of the vehicle calculated by the second arithmetic means, and a vehicle position and attitude angle measuring device comprising: . 2. A storage means for sequentially storing the calculated xyz coordinate position of the representative point of the vehicle, and the storage means.
The vehicle position and attitude angle measuring device according to claim 1, further comprising a map output means for creating and displaying a map based on the xyz coordinate position. 3. The vehicle position and attitude angle according to claim 2, wherein said storage means stores only the latest three-dimensional position in the case of three-dimensional positions having the same xy coordinate position. Measuring device.