JPS62273408A - Measuring apparatus of vehicle position and posture angle - Google Patents

Abstract

PURPOSE:To enable realtime measurement with high accuracy at a construction work site with frequent changes of the road surface shape, by installing two light-flashing means set at two constant points, two light-receiving means arranged on a vehicle, and detecting means for a vehicle inclination angle. CONSTITUTION:xy co-ordinate positions of a light-receiving position at an apparatus 2 are obtained and also those of the apparatus 3, based upon a setting distance between light-flashing apparatuses 11, 12 and two rotating angles of a laser beam when the light-receiving apparatus 2 received each rotating laser beam from the apparatuses 11, 12. And left and right inclination angle of a vehicle is detected directly from a left and right inclination sensor 4 and fore and aft inclination angle and direction angle are obtained based upon the setting distance of the apparatuses 2, 3, light- receiving height position detected by the apparatuses 2, 3 and xy co-ordinate position and inclination angle previously determined. Next, xy co-ordinate position of a vehicle representing point is obtained based upon one of the xy co-ordinate positions of the apparatuses 2, 3, and inclination angle and direction angle detected or calculated and the z-co-ordinate position of the vehicle representing point is obtained by the z-co-ordinate position of the rotating laser beam, light-receiving height position of one of the apparatuses 2, 3 and inclination angle of them.

Description

[Detailed Description of the Invention] 3. Detailed Description of the Invention [Field of Industrial Application] This BA relates to a device for measuring vehicle position and attitude angle, and is particularly applicable to driving support for construction vehicles operating at construction sites. A total of 6 vehicle positions and attitude angles to obtain information such as the current position of the vehicle and the topography of the site, which are necessary when automating work.
:Relating to II device.

[Conventional technology]

Conventionally, as a system that automates the driving support and cutting of work vehicles at construction sites and then measures and utilizes the three-dimensional position of the vehicle, Japanese Patent Application Laid-Open No. 59-180422 (travel trajectory analysis system) can be cited.

These seven stems sample the vehicle running speed, vehicle longitudinal inclination angle, and running direction at regular intervals, and calculate the current position of the vehicle at regular intervals based on these sampled values.

[Problem that the invention seeks to solve]

However, the above-mentioned E and the two position meters are not correct! In the case of one system, the vehicle's current position t? is calculated by accumulating the relative movement amount of the vehicle. g
There is a problem in that a cumulative g4 difference occurs in the current position of the vehicle due to irregularities in the road surface, 'j@, wheel slip, or errors in one sampling shift, resulting in a 5 degree error in vehicle position measurement.

The present invention was developed in view of the above-mentioned circumstances, and is capable of measuring the three-dimensional position of a vehicle and the attitude angle of the vehicle in real time with high precision even at construction sites where the shape of the working road surface changes frequently. An object of the present invention is to provide a vehicle position and attitude angle measuring device.

[Means for solving the gap]

In order to achieve the above object, the present invention has 92 settings in advance.
The first laser beam is installed at a fixed point and emits a rotating laser beam that rotates at a constant period. A second light projecting means, a second light receiving means disposed in the vehicle (n1) detecting the light receiving timing and light receiving height position of the rotating laser beam; tilt angle detection means for detecting the #1$Ir angle of 1; 1st゜2nd
1r[1,
M3 of the rotating laser beam from the light projecting means. g4; rotation angle W output means for determining the rotation angle of rotation angle W; The first. the second rotation angle and the third rotation angle. A first method for calculating a first xy coordinate position and a second xy coordinate position Jd on a certain xyZ coordinate system of each light receiving position in the first and second light receiving means based on the fourth rotation angle. arithmetic means, and the first
．． The distance between the second light receiving means, the light receiving height position detected by the light receiving means, the first tilt angle detected by the tilt color detecting means, and the first tilt angle calculated by the first calculation means. 1. A second calculation means for calculating a second angle of inclination and a ten-thousand angle in the front and back or left and right directions of the vehicle based on the second xy coordinate position; at least one light receiving height position detected by the second light receiving means;
At least one xyFE calculated by the calculation means of
Third calculation means for calculating the xyf% target position of the vehicle representative point based on the target position, the longitudinal inclination angle, the left-right inclination angle, and the azimuth angle of the vehicle detected or calculated by the inclination angle detection means and the second calculation means. and the two coordinate positions (medium) of the rotating laser beam, at least one of the light receiving height positions detected by the first and second light receiving means, the ejection or calculation by the inclination angle detecting means and the second calculating means; The present invention is characterized by comprising a fourth tiI calculation means for determining the two coordinate positions of the two representative points based on both longitudinal and lateral inclination angles and lateral inclination angles.

[For production]

First, the above-mentioned 1. The installation interval of the second light projecting means and the light receiving means of ml are the same as the first. The first rotating laser beam when each rotating laser beam is received from the second light projecting means. Based on the second rotation angle, the light receiving position of the first light receiving means is determined by the principle of triangulation. The first xy coordinate position on the 72 coordinate system is determined. Similarly, the xy position of the light receiving position in the second light receiving means (2) is determined.

Since the vehicle is not always in a horizontal state, it is not possible to immediately determine the X'f coordinate position of the vehicle representative point from the xyy marker position. Therefore, the attitude angle of the vehicle (vehicle longitudinal inclination angle, lateral inclination angle, and azimuth angle) is determined.

One of the longitudinal and lateral inclination angles of the vehicle is directly detected by the inclination angle detection means, and the other inclination angle and azimuth are detected by the first. The arrangement interval of the second light receiving means, the light receiving height detected by the light receiving means, and the tilt angle detected by the tilt angle detecting means and the first and second xyy target positions are determined.

The xy coordinate position of the vehicle representative point is at least one of the first and second χy coordinate positions xy! IK! Camphori rk
't, the above-mentioned detection, i-t, and x, and the two-coordinate position of the vehicle representative point is determined based on the two-coordinate position of the rotating laser beam. It is determined based on at least one of the light receiving height positions detected by the second light receiving means and the attitude angle of the vehicle (excluding the auxiliary and azimuth angles).

〔Example〕

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

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

In Figure 2 IC, the distance between points A and B is the angle between the LXx axis and the line segment AC, and the angle between XU and the line segment BC is αb.
Then, the xy coordinates of point C can be expressed by the following equation. Therefore, in the present invention, the above α8. When calculating αb, a light projector that projects rotating laser beams is installed at points A and B, respectively, and a vehicle (equipped with a light receiver) receives the light from either this light receiver or the rotating laser 5. The purpose is to detect the rotation angle of each rotating laser beam.

This rotation angle α1. αb is w, the period of the rotating laser beam is T (Fig. 3(, )), and points A and B are
The time from when each rotating laser beam from the light installation device installed at the point reaches the reference direction (X-axis direction) until it enters the single-mounted light receiving device dK is t and J, respectively.

(Fig. 3(b) and (C)), it is calculated as follows.

The xy coordinates calculated by the above equation (1) are the coordinates of the laser beam receiving point of the single light receiving device and are not the coordinates of the representative point of the vehicle. Therefore, when converting to the coordinates of the vehicle representative point, the attitude angle of the vehicle (vehicle longitudinal inclination angle, lateral inclination angle,
azimuth angle).

Next, a method for determining the attitude angle will be explained.

First, consider modeling the vehicle body 1 and the light receiving device 2.3 as shown in FIG. Here, point P is the origin of the coordinates of the tx'y'z' coordinates, and the point x of each part of the vehicle body shown in FIG. The coordinate position in the /y/s/ coordinates is shown in FIG.

Note that point Q and point T are laser beam receiving points in the light receiving devices 2 and 3, respectively.

Now, the movement position of the Q point and T point in the xt y/z/ coordinate system when the attitude angle of the vehicle body changes (
Xqe Fq+ Zq ) and (4t* 3'te
Zt) is expressed by the linear equation,

Therefore, the position of point P in the xyz image system with AA in Fig. 2 as the origin is f &'t'' P (Xg + )'6
m'10), the position ()Cq+3'qsZq× and (X*)'l+''t) of the QAZNT point in the xyz tilt system is expressed by the following equation, where the (5th ) formula, (xq * in formula (6)
74 ) and (xt+yt) are respectively the above-mentioned (
1) Jl can be calculated based on the formula. However, either one of the longitudinal inclination angle θ and the left-right inclination angle δ of the vehicle, and in this embodiment, the left-right inclination angle δ is measured using a tilt sensor.

From equations (3) to (6) above, = (h, -h2) part δ 0(2)ψ-h1gIn'
ghiψ−baisθcaψ10. (7) = (h, -
h2) (2) δ gallery θ gallery ψ + h1 − δ ■ ψ − b part θ − ψ ... (8) If θ is eliminated from the above equations (7) and (8), h, I
IIIδ= (7q −y t ””ψ−(xq−x
t) thψ=U癲(ψ+U) ...(
9)xq-xt, and when the Joki-Zheng (9) equation is transformed, it becomes. Also,
When we solve equation (7) for θ, we get a friend, u' =
, / (h1-h2)2as2δas2(p+b
2cos29). In other words, the measured (Xq
+3'q) + (Xt*yt) and δ, the azimuth angle ψ and longitudinal inclination angle θ of the vehicle can be determined using the above four holes and Equation 09.

When the above attitude angles (θ, δ, ψ) of the vehicle are determined, the position of point P (x0*y6*z□) is determined by g (3
) and equation (5), the following equation
廁6(2)ψ)16 =Xq-111QJsl-〇=h
, -hjcr15δmorning θ...(2). This is the height of the rotating laser beam in the xyzr standard system.

Therefore, from the fourth equation, the place f of point P? It is also possible to find the position of the vehicle representative point (for example, the center of the east) that has a certain relationship with the point P.

FIG. 1 is a block diagram showing one embodiment of the present invention. In the figure, the ground station side is composed of 1012 ground reference stations, projecting devices 11 and 12, a map display 13, and a map data storage device 14. The ground reference station 10 further includes a transmitter/receiver 10a1, a processing device 10b, and an oscillator 10c, and emits light! 4th position 11.12 is the pulse motor",
12, rotary heads llb, 12b, and laser oscillators 11e, 12c.

On the other hand, the vehicle includes two light receiving devices 2.3, a vehicle left/right tilt sensor 4, and a transmitter/receiver 5i? In addition, there are devices for calculating, processing, and controlling the ridge hammer.

Floodlight fii! :11 and 12 are installed at points A and B in the xyz coordinate system with a constant interval as shown in FIG. Here, the rotating laser light plane from the light projector 11°12 is! Each light beam is rotated so that it is parallel to the y-plane, and the height of the light plane is set to a predetermined height ho.

The light projecting device 11 inputs a laser beam that is continuously emitted and lost from a laser oscillator 11a into a rotating head 11bK, and rotates this rotating head 1lb(i-)#sysmo 211m to emit a rotating laser beam from the rotating head flb. Emits light. Pulse length 11m is ground reference station 10 emitter 1
It rotates in synchronization with the clock pulse applied from 0a. Note that the light projecting device 12 also projects a rotating laser beam in the same manner as the light projecting device 11. That is, the rotational synchronization and phase of the rotary heads llb and 12b are made to match.

Further, the processing device 10b of the ground reference station 10 includes an oscillator 10
The floodlight device 11.12 by the clock pulse from c.
The point in time when the laser beam from the sensor points toward the reference direction (X-axis direction) is detected, and at this point, the transceiver 10m generates a reference direction signal indicating the reference direction.

3, the Makoto transmitter/receiver 5 receives the reference direction signal f.
This is added to the silver image position calculation device 20. Further, the vehicle position calculation device 20 receives signals from the light receiving devices 2 and 3 indicating the timing of receiving the rotating laser beams, and the light receiving height positions h1 and h2t? A signal indicating the left-right tilt angle δ is added to the history from the vehicle left-right tilt sensor 4. If the vehicle side is provided with a means for measuring the time of the oscillator 10a on the ground station side and the ground station side, the point in time when the laser beam is directed toward the reference direction can be detected only on the vehicle side.

Vehicle position calculation system [20 minerals, firstly the light receiving position of each light receiving device [Q
, the coordinate position tR(Xq+ 7qe Zq) of T and C
Xt* 1% r Zt) in equation (1) and g (2
Calculate based on Equation 1. Note that x, = Q = ho. Next, in this way, find 9 ("q * 7q
), (xt, yt), the light receiving height position h, and the slope angle δf from the slope sensor 4! :g (Determine the azimuth angle ψ by substituting it into equation 10.Furthermore, each value above and the azimuth angle ψ
By substituting the light receiving height 11th2 into the 0uth equation, the vehicle longitudinal inclination angle θ is obtained.

Next, the above light receiving position fQ (X4.y, #Z,) and T
The three-dimensional position of the vehicle representative point is determined based on (Xt*Yt+zt) and the attitude angle (θ, δ, ψ) determined by K as described above. Note that the position #P (X6 e 76 * ”6) of the P point of the vehicle attachment part of the light receiving device 2 is obtained by substituting (Xqs 7q + Zq) and (θ, δ, ψ) into the αth equation. be able to.

The three-dimensional position of the representative point of the original image obtained in this way.

A signal indicating this is applied to the map data processing device 21. Map data processing device! lt, 21 is the map data storage device 2
New map data is created by updating the vehicle travel trajectory, topographic data, etc. based on the old map data from 3.

For example, the driving area (work area) of a vehicle is divided into a finite number (nXn) in the When the vehicle reaches a certain position, the map data of the three-dimensional travel trajectory can be stored by inserting two digits into the storage unit corresponding to that position.

Table 1 Note that if it is too late to reach the position where the two seated images have already been placed, the system will be used to change the coordinates to the latest two coordinates.

If the representative point of the vehicle is the point of contact with the ground, the three-dimensional travel locus data stored as described above can be regarded as terrain data.

! The map data processing system! 21 indicates map data, Q
Based on the map data stored in the device 23, the vehicle's travel trajectory or topography is displayed on the map display 22.

FIG. 6 shows an example of a map display. You can also display the current location of Mochi Chest and the original picture on the map.

! The on-board transceiver 5Vi transmits the map data stored in the map data storage device ItK, and the transceiver 10& of the ground reference station 10 receives it 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 displays the travel trajectory or topography of the vehicle on the map display 13 based on the stored map data.

Scheduled work content is set in the work order generation device 24, and the arithmetic processing unit j125 receives a signal indicating the work content added from the work order generation device and data (map data) added from the district data processing device 21. , claw side position data, attitude angle data), the engine control signal, shift control signal, steering control signal, and work equipment f
The lill m signal is generated to control the engine control device 26, the transmission control device 27, and the steering ItI11 control device #, 28.
and controls the machine control device 29.

Note that the above system obtains vehicle position data, terrain data, and work commands in real time and communicates them with the ground reference station 10 to remotely control the vehicle. It is possible to configure a control system for continuous construction work that performs 1j control, etc.

In addition, in this embodiment, the KLI uses the above-mentioned equation (5) and equation (
6) 弐【, respectively @ The equations in which equations (3) and (4) are substituted are xq= x. + h1cosJfnθcm9+ -h1
magin9+7q = Y0+hlaBδml φ+
h1mδatsqJzz = go+ h1cosJe
atθ
[Yes] ψ7t” 7 o + h l CIRδgfa
#*(P + 1) coso ψzt= z0+h2c
ce+δ. The sum of θ- and +ttnθ results in 1i6 equations, and the unknowns in these equations are (16m 7G116 ) and (δ, θ, ψ), a total of 6, so all unknowns can be found by solving the above 6 equations. can. Therefore, the #ix sensor is not suitable for this situation.

〔Effect of the invention〕

As described above, according to the present invention, the :E, dimensional position of a vehicle can be determined accurately and in real time without accumulating measurement errors even in deserted areas such as construction sites.

In addition, Ii r#fi! Sofl! Information and topographical information) can be provided to confectioners to support their creation.

[Brief explanation of the drawing]

FIG. 1 is a block diagram showing one embodiment of the present invention, and FIGS. 2 to 5 are for explaining the position*i measurement method according to the book
This figure shows an example of the display of the map display in Figure 86 and Figure 1 used. DESCRIPTION OF SYMBOLS 1...Main body, 2.3...Light receiving device, 4...Vehicle left and right inclination sensor, 5...Transmission/reception aircraft, 10...Ground reference station, 11.12...Light projector, 13 .22...Map display, 14.23...Map data storage device, 20.
... Vehicle rank 1J! t, calculation device, 21... map data processing device. To the Departure Personnel Representative Hisashi Kimura Tsutsu Figure 2

Claims (4)

[Claims] (1) First and second light projecting means installed at two preset fixed points and projecting rotating laser beams that rotate at a constant cycle; first and second light receiving means for detecting a light receiving height position; a tilt angle detecting means for detecting a first tilt angle on either the left or right side or the front or back of the vehicle; Based on the light reception timing, the first and second rotation angles of the rotating laser beams from the first and second light projecting means and the second rotation angle at the time of light reception are determined.
rotation angle calculation means for calculating third and fourth rotation angles of the rotating laser beams from the first and second light projecting means at the time of light reception based on the light reception timing detected by the light reception means; , each of the first and second light receiving means based on the installation interval of the second light projecting means and the first and second rotation angles and the third and fourth rotation angles calculated by the rotation angle calculation means. a first xy coordinate position and a second xy coordinate position on a certain xyz coordinate system of the light receiving position;
an arrangement interval between the calculation means and the first and second light receiving means;
Based on the light receiving height position detected by the light receiving means, the first inclination angle detected by the inclination angle detecting means, and the first and second xy coordinate positions calculated by the first calculating means, a second calculating means for calculating a second left and right inclination angle and an azimuth; at least one of the light receiving height positions detected by the first and second light receiving means; A third method for calculating the xy coordinate position of the vehicle representative point based on the one xy coordinate position, the longitudinal inclination angle, the lateral inclination angle, and the azimuth angle of the vehicle detected or calculated by the inclination angle detection means and the second calculation means. a calculation means, a z-coordinate position of the rotating laser beam, a light reception height position of at least one detected by the first and second light reception means, a vehicle detected or calculated by the tilt angle detection means and the second calculation means; a fourth step of calculating the z-coordinate position of the vehicle representative point based on the longitudinal inclination angle and the lateral inclination angle of the vehicle;
A vehicle position and attitude angle measuring device comprising: calculation means; (2) storage means for sequentially storing the calculated xyz coordinate positions of the representative points of the vehicle, and xyz stored in the storage means;
A vehicle position and attitude angle measuring device according to claim 1, further comprising map output means for creating and displaying a map based on coordinate positions. (3) The vehicle position and attitude angle measuring device according to claim (2), wherein the storage means stores only the latest three-dimensional position in the case of three-dimensional positions having the same xy coordinate position. . (4) first and second light projecting means that are installed at two preset fixed points and project rotating laser beams that rotate at a constant cycle; first and second light receiving means for detecting a light receiving height position; and rotating laser beams from the first and second light emitting means at the time of light reception based on the light receiving timing detected by the first light receiving means. the first and second rotation angles of and the second rotation angle of
rotation angle calculation means for calculating third and fourth rotation angles of the rotating laser beams from the first and second light projecting means at the time of light reception based on the light reception timing detected by the light reception means; , each of the first and second light receiving means based on the installation interval of the second light projecting means and the first and second rotation angles and the third and fourth rotation angles calculated by the rotation angle calculation means. a first xy coordinate position and a second xy coordinate position on a certain xyz coordinate system of the light receiving position;
an arrangement interval between the calculation means and the first and second light receiving means;
the light receiving height positions detected by the light receiving means and the first and second x calculated by the first calculation means;
a second calculation means that calculates the longitudinal inclination angle, the left and right inclination angle, and the azimuth angle of the vehicle based on the y-coordinate position; and at least one of the light receiving height positions detected by the first and second light receiving means; A third method for calculating the xy coordinate position of the representative point of the vehicle based on at least one of the xy coordinate positions calculated by the first calculation means and the longitudinal inclination angle, the left and right inclination angle, and the azimuth angle of the vehicle calculated by the second calculation means. a calculation means, a z-coordinate position of the rotating laser beam, at least one light reception height position detected by the first and second light reception means, and a longitudinal inclination angle and a left and right inclination angle of the vehicle calculated by the second calculation means; A vehicle position and attitude angle measuring device, comprising: fourth calculation means for calculating the z-coordinate position of the vehicle representative point based on;