JPS59180422A - Running-locus analyzing system - Google Patents

Abstract

PURPOSE:To analyze the three-dimensional running locus of a vehicle accurately, by detecting three kinds of data of the vehicle speed, running path gradient, and running bearing of the running vehicle only by running the vehicle on the spot, and continuously detecting the present position of the vehicle based on the data and the running distance of the vehicle. CONSTITUTION:When a vehicle is made to run on the working spot and a system is started, a vehicle speed V, a running bearing angle gamma, and a vehicle slant angle theta are individually detected by sensors 1-3. The three detected analog signals are converted into digital values by A/D converters 4-6. The converted values are expressed by 2's complement in 12 bits and sampled by a CPU7 at every specified time interval. The CPU7 reads a data table corresponding to the inputted sample value from an ROM9-1, and executes the operation for analyzing the running locus of the vehicle working on the spot. Thus the result is outputted. Namely, the system executes the cycle of sampling of the speed V, the slant angle theta, and the bearing angle gamma of the running vehicle operation based on the sampled value outputting of the result of the operation.

Description

DETAILED DESCRIPTION OF THE INVENTION The present invention relates to a traveling JL track analysis system suitable for improving running routes for vehicles such as construction machinery.

For example, when operating a work vehicle such as a loader or dump truck at a plant or dam construction site, it is possible to measure the route or travel trajectory of the vehicle in advance to improve the route, improve fuel efficiency, and work to suit the terrain of the route. Many benefits can be obtained in terms of specifications, vehicle model selection, etc.

However, the reality is that there is still no generalized and simple method for this type of measurement.

In detail, topographic surveying is extremely common as the only method to measure the topography of the driving route mentioned above, but this method requires a lot of time and effort for manual surveying on site and data processing of the results. This results in spending .

For this reason, it is not possible to grasp the topography of the vehicle running route in a short time, and due to this, it is not only necessary to improve the track, but also to improve the work specifications for the track, the selection of vehicle models, and the improvement of driving operations that match the track topography. A major drawback is that advice cannot be provided immediately and appropriately.

Moreover, no simple method has been found to measure the trail of Mitt's travel.

This invention was newly created as a result of intensive research in view of the above circumstances.

The main object of the present invention is to immediately analyze the three-dimensional travel locus of a vehicle as it travels on site.

Another object of the present invention is to make it possible to quickly grasp the topography of running routes for working vehicles at various plants, dam construction sites, and the like.

Another object of the present invention is to select work specifications and vehicle models that suit the road topography, and to provide appropriate advice, improvements, and changes in vehicle driving operations.

Still another object of the present invention is to enable the current position of a traveling vehicle in a field to be monitored at any time.

Still another object of the present invention is to enable safe unmanned running of a vehicle on a road with a terrain G.

This invention is capable of three-dimensionally analyzing a vehicle's traveling trajectory from time t=Q to the present after t=Tsec based on three types of detected data: the vehicle's traveling speed, inclination (travel gradient), and traveling direction. It is characterized by the following.

Hereinafter, preferred embodiments of the present invention will be described based on the drawings.

The system of the present invention includes a microcomputer that is installed in various work vehicles such as loaders and dump trunks, or on-site measurement vehicles.

As shown in Figure 1, the hardware consists of three types of sensors 1 to 3 that individually detect the speed, heading, and slope angle (road slope) of the vehicle, and an analog signal input from the sensors. A/l) that converts the signal into a digital signal
Converters 4 to 6, and an arithmetic mechanism (hereinafter referred to as "arithmetic mechanism") for inputting digital signals from the transducers and reading out corresponding arithmetic expressions (programs) from the memory 9 to calculate the position of the traveling vehicle on three-dimensional coordinates. 7 and a D/A converter 10 that converts the result into an analog quantity and outputs it.
- 12, and an output device 17 that draws the traveling trajectory of the vehicle based on the output data from the converter.

More specifically, the speed sensor 1 consists of a rotational speed sensor that detects, for example, the engine rotational speed and the rotational speed of a wheel or axle using an electromagnetic pickup method when the vehicle is running.

The force position angle sensor 2 is composed of a course gyroscope or a geomagnetic sensor, and detects the angle in the traveling direction of the vehicle (the amount of change from the initially set position).

The tilt angle sensor 3 includes a vertical gyroscope and other sensors for measuring the tilt of the vehicle with respect to the vertical axis.

Cl) U 7 includes an interval timer 9 for sampling data signals from each sensor 1 to 3 via A/D converters 4 to 6 at fixed time intervals, and peripheral equipment (not shown) (not shown). Riya Slam Controller)
It is equipped with

The memory 9 consists of a ROM 9-1 in which a program with contents to be described later is written, and a RAM 9-2 used as a temporary storage and a stat for variable data.

The ROM 9-1 in the illustrated example is composed of 16 high chips (8 high chips per 2 chips), and chips are selected by decoding the address bus.

In the ROM 9-1, the main program is written in the area 0 to 2, the subroutine is written in the area 12 to 14, and the Sin table is written in the area 14 to 16.

On the other hand, RAM9-2 is a 4-bit XIK high I chip 2
It is composed of individuals.

In addition, the reference numeral 13 in FIG. 1 is the address decoder of the memory 8, 14 is the address decoder of the A/D converters 4 to 6, and 1 is the address decoder of the memory 8.
5 is an address decoder for the D/A converters 10-12.

The system configured as described above analyzes the three-dimensional travel locus of the vehicle each time the current position on the three-dimensional coordinate system is calculated based on the travel distance of the vehicle.

The flowchart I of the entire program in this case is shown in FIG.

That is, when the system is started when a vehicle is running at a work site, the speed V and running azimuth angle γ (second
(see figure) and vehicle inclination angle θ (see figure 3) are individually detected by each of the sensors l to 3.

The detected 3 analog data signals are transferred to the A/D converter 4.
.about.6 are input and converted into digital data values, respectively. The converted value is expressed as a 12-bit two's complement number, and C
The PU7 performs a sampler at regular intervals.

Here, regarding the interrupt, the interrupt is a request from the sampling interval timer 9, and the interrupt time can be converted using an i-hex digital switch. When the CPU 7 is interrupted, the program jumps to address 56.

Cl) U7 is 6ii for extracting the data table (Sin table) corresponding to the human power sample value from ROM9-1 and analyzing the travel trajectory of the vehicle operating on site.
performs the calculation and outputs the result.

In other words, the system of the present invention executes a cycle of sampling the speed (1), inclination angle .theta., and azimuth T of the vehicle, calculations based on the sampled values, and output of the calculation results.

The output terminal of the interval timer 9 for repeatedly executing this cycle is connected to the interrupt request terminal of the CPLI 7. Therefore, C1)U7 executes the above cycle when an interrupt occurs, and enters a waiting loop until the next interrupt occurs.

In this way, the interval timer 9 repeatedly executes the cycle of 2 times every predetermined period of time Δ.

In the calculations during the above cycle, addition and multiplication are performed by floating point/Iij calculations by software, and calculations of trigonometric functions are performed by drawing out a Sin bull corresponding to the input sample value.

Therefore, the output of the calculation result from the CPU 7 is converted into three-dimensional coordinates (
X-Y-Z), the velocity components in the X, Y, and Z axis directions are determined by the following formula: Vx, V, y, Vz &.

Vx = V-Cosθ・Cos r -----------(
1) Vy = V -Co5O-3inT----
(2) Vz=V・Sinθ −−−−−−−(3
) Sin and CO in these formulas (1) to (3)
The volume of 3 is determined from the Sin table written in advance in the ROM 9-1.

Therefore, the CPU 7 reads the Sin table from the ROM 9-1 when sampling the digital data I1 negative θ via the Δ/I) converter 6 of the 3 tilt angle sensors and their system, and reads the Sin table from the ROM 9-1 and uses the above formula from the table. When Sin θ and Cos θ in (1) to (3) and the digital data value T from the system of the azimuth angle sensor 2 are sampled, 5 inr and Cos γ are obtained from the above Sin table.

Here, to describe the Sin table in detail, the table is located in the area number 2 of ROM9-1 at /\ite.
Divide the area into 1024 equal parts and divide 2 pieces into 3 inches (i*
is expressed.

To explain in more detail, ROM9-1 has 1-,
In the area of even addresses, the mantissa's ``binary) person'i1-force (
, 1:1. In the area of odd number Hyde, there is the lower Bi/Nod of the mantissa and the number 5 Bi of the loaf.

Soto is included, part 2) λi I-X 1024.”
A second Sin table is constructed.

In this table, the range of 0≦α<te of the sin wave as shown in Fig. 4 is divided into 1024 equal parts, and the Sir of α-〇 is calculated.
α is calculated by π/2048 increments from the +α value, G = 1023
/2048π up to the Sin α side is included.

Therefore, the sin wave (sin table) in Fig. 4
To find 3inα from , the α value must be addressed as shown in Table 1).

As is clear from Table 1, the 7-trace decoder 1 in Figure 1
R of the Sin table in 14K ~ 16K by 3
OM! Address A1 to select chip -1
1 to Δ13 must all be 1. Those An
~A, In order to enable chip selection with 13, Al
1. A15 must be 0.0. Also, A1~
AIO is used for addressing where α is located in 1024 equal parts. In this case, if AO is 0, Si
If the upper mantissa hide of the nα value, AQ, is 1, the lower 3 bits of the mantissa and 5 bits of the exponent are accessed.

Therefore, the data input (travel slope) θ detected by the inclination angle sensor 3 is converted into a 12-pips 1-2's complement digital quantity by the A/D converter 6 of that system.

Read the above Sin table from the converted value θ' and
In order to obtain inθ゛ (-sign≦θ<+), it is necessary to address the digital amount of θ.

However, if the digital quantity (12-bit two's complement integer of θ) of 1 order≦θ<te is used as an address as it is before then, the result will be as shown in Table 4. In this case, AI2~Ats
are all zero.

Table 2 Here, the waveform of Sin θ shown in FIG. 5 and the waveform of the Sin table shown in FIG. 4 will be compared.

(1) When 0≦θ<π/2, the Sin of the Sin table
The waveform and the waveform of Sinθ match. Therefore, the addresses in Table 2 and (A15. A14. A13.
A12. An) = (0,0,1,l,1)
The value of Sin θ is obtained by ORing the mantissa minus the upper focus of Sin θ when A0=0, and accessing the lower 3 bits of the mantissa and 5 bits of the exponent when Al=1. In this case, AIO to A1 remain as shown in Table 2.

(11) - When 〼≦θ〈0, take the complement of the 12-bit two's complement integer θ゛ and make it into the waveform of ■ in Figure 5, AQ
= 0, 80 = 1, 5 inches (-θ) from the Sin table,
Find the value of .

However, this value was obtained by using the Sin table from the waveform .

Therefore, in this case, take the two's complement of 5in(-θ) and ζ
-3 inches (-θ). In this case, the two's complement is Sin
This is done by expanding 2 bytes of 5 inches (-0) found from the table to 3 hides of 5 inches (-θ).

That is, when -arrow≦θ<0, it is determined by -3 inches (-0).

In (i) and (ii) above, it is necessary to determine whether O≦θ×1σ− and 1≦θ<0 (calculated by Sinθ or −3in(−θ
) in Table 2, Pinot Ill is 1 or 0. That is, when the bit is 11 or 1, it is determined by -3in (-θ), and when it is 0, it is determined by sinθ.

θ here is shown in Table 2 and is not addressed for the Sin table, so the S of 2 Hyde is
Convert the inθ value to 3-hyde sinθ.

FIG. 10 shows flowchart 1 for determining Sin θ (-sign≦θ<+).

Next, based on the input sample value θ from the three systems of tilt angle sensors, the Cos in the X and Y axis directions is calculated from the Sin table
The case of determining θ ri−kidney≦θ<+) will be explained in detail.

(i) When +≦θ〈0, Table 1 shows that one −≦
AO to Δ1o of θ<0 and 0≦θ<÷ are the same table values. This means that the phase of θ of −9−≦θkuo is divided by
This means that AQ~Δ1o will not change in any way even if the process is advanced.

Here, if we look at the waveform of Cosθ (-s≦θ<0) shown in Figure 6, when -4 to ≦0<0, the waveform becomes ■, but in this case, even if we advance ten phases, Since AO to A10 do not change in any way, there is no problem in using the Sin table.

That is, since the waveform of ■ and the sin wave ■ of the sin table match, Cos θ is determined by Sin θ.

In this case, (A15. A14. A13. Al1.
A11) = (0,0,1,1,1) and A of θ 10
Sinθ is obtained from the Sin table by ORing with ~AO and accessing with AO=0.1. The resulting value becomes the value of Cos θ.

(ii) When 0≦θ<9, the waveform of Cos θ becomes ■ in FIG. For convenience, use Cosθ=3in (+-〇).

In this case, the A/D converter 6 converts the analog input (1÷≦θ) into a 12-bit two's complement integer. Therefore, the .theta.-number cannot be represented by a 12-bit two's complement integer. To express this, θ in Table 3 is
The item sequence is -÷---1.

Table 3 In Table 3, if θ of laziness is compared to its item string, the 12-hit integer of (+-θ) becomes like the item string of ÷-θ in the same table. In other words, (small −〇)(0≦θ〈+
) can be obtained by taking the two's complement of θ.

Therefore, when 0≦θ, Cos from the Sin table
When calculating θ, take the complement of O. When θ-〇, it becomes (+-θ)-te and Sin+ is written in the Sin table.
does not exist, so in this case, in the Sin table, S in (1023/2048) π-1, so = == (1023/2048) is approximated as 5
Find in-ji-=1.

If θ=0, take one's complement. And Δ of θ
OゞAIO and (ΔIt, A12. A13.
A14. By taking the OR with A15) = (1,], 1.0.0), Sin
Find the 2-byte Sin value from the table. its value is 0
≦θ<Cosθ at the time of fang. Then 2 bytes of C
Expand osθ by 3 pi l−4.

2. When calculating Cosθ, (i) and (ii) are summed up. When -1-min≦θ<0, Cosθ is found by Sinθ<m2 (bit 11 or 1 in the table), O≦ Cos θ when θ ÷ is determined by Sin θ (Bit j1 in Table 2 is O) The direction of the origin on the three-dimensional coordinates, for example, the direction of the vehicle's progress based on the X-axis direction on the X-Y plane. The direction γ is detected by the force position angle sensor 2 and then converted into a 12-bit two's complement integer by the 8/I) converter 5. The case where Sin γ is calculated from the converted γ value using the Sin table will be explained in detail below.

In this case, it is necessary to address the digital amount of γ converted by the A/D converter 5 as described above. Before that, if the digital quantity of -π≦γ<π is shifted to the left by 1 bit and used as the address of AO-A15 (A13 to A15 are crossed and set as the opening), Table 3 is obtained, and Si
When the waveform of nγ (-π≦γ<π) is plotted, it becomes as shown in FIG. 7.

I will explain it in succession.

First, in the case of the waveform curve ■ (Sinr=-π≦r<H), looking at Table 4, even though the phase of -π≦T -÷ is shifted by π, the address of A1-AIo remains the same. .

Therefore, since the digital quantity γ shifted to the left by 1 bit of the -π≦γ<- bone and the 1-bit left digital quantity γ of the -π≦1<- match, Al''Ato is shown in Fig. 7. Consider the waveform of ■ as ■゛.

Then, as is clear from the figure, when -π≦T<-9, 13
It is determined by inγ. The value of Sinγ in this case is Sin
It is a value from a table. In other words, the addressing of γ is (A11, A12, A13, A14, Al5) = (1
, 1, 1, 0, 0) and the digital quantity γ (values in Table 4) shifted by 1 bit to the left to obtain the 3in7 mantissa "1".
The lower 3 bits of the mantissa and the 5th exponent are accessed as AO-1. Then S from the Sin table
-Sin[gamma] when -π≦T<4 can be obtained by expanding the value of in[gamma] to 3 Hyde and taking two's complement.

In the case of the waveform curve ■ (Sinr=-÷≦T<0), T is the value in Table 4, which is shifted by 1 bit to the left, so
Do the same as in the case of Sinθ. In other words, using 5 inr of 0≦γ in the Sin table, −3−≦r<0
When calculating the value of 5inr, calculate it as -5in (-γ). That is, γ shifted left by one hint (values in Table 4)
In this case, since it is left-softening, the two's complement is taken as A1-A12 (one's complement when -chi γ), and then addressing A11-A15 for the Sin table and Ao = Find the direct value of 5in (-γ) with 0.1, extend it to 3 bytes, take the two's complement of 5in (-T), and set it as -5in (-7).

In the case of the waveform curve ■ (0≦T<+), 5in7 is obtained by addressing γ for the Sin table.

In the case of the waveform curve ■ (+≦γ<π), as is clear from Table 4, even if nine phases are delayed, the addresses A1 to AIO match, so that it can be replaced with the waveform curve ■゛ in the figure.

Therefore, in this case, the waveform curve ■−■゛ is 5 inches (
+−γ) to find Sinγ.

In other words, 5in(-;-7) in this case is C
Same as in the case of osθ (so 5in(chiγ)−3
It is determined as in(-γ).

That is, Sinγ is the two's complement of the T value in Table 4.

However, when γ=, one's complement is used. Because S
Since there is no Sin+ in the in table, 5in
(1023/2048) Take one's complement as π-1.

After that, address AI1 to AI5 for the Sin table and set AO=1. Find 5 inches (-r') as O.

Here, looking at the addresses All and A12 in Table 4, we see that A
11=1 (range of +≦γ〈π, - sign≦γ〈0), 2
Take the complement of (1's complement for Te and - signs) and make T a negative number. Also, A12=1 (−π≦γ<−, −逼≦r<
0 range), if the Sin value is 2's complement, it is made negative.

Therefore, Sinγ is All of T shifted to the left by 1 bit,
The method of determination is determined by A12.

Second, FIG. 11 shows a flowchart for determining Sinγ (-π≦T<π).

Next, a case will be described in which Co5T is calculated from the Sin table based on the input sample value from the azimuth sensor 2.

In this case, as is clear from Table 4, the maximum positive number hit 1
When 1 is added to , it becomes γ-π, and T matches the value of -π. Therefore, when the phase of Co5r is advanced by TE, <Sir+r is obtained as shown in FIG.

Therefore, when calculating the value of Co5T for -π≦T<π from the Sin table, the phase of T in Table 4 is the sign, that is, All
All you have to do is add 1 to
Since it matches nr, after that 1π≦T×π 5inT
You can do the same as.

In this way, the CPU 7 reads 120M9- based on the data values sampled from the respective systems of the sensors 1 to 3.
Sin and C of the above equations 1 to 3 from the Sin table of 1
Find the value of os. Then, multiple calculations are performed, and the resulting values are each integrated over time -CX.

Calculate the current position of the vehicle in Y and Z.

Integration in this case is performed by a quadratic approximation method in order to minimize integration errors in calculations.

Therefore, the sampling interval by the interval timer 8 is Δ, the velocity corresponding to the sample values of V, θ, and γ at time T is Vn, the current position at that time is βn, and the time T is one sampling before time T. - Velocity at Δt is Vn
-1, the current position at that time is βn-1, the speed at time 'F-2 Δt 2 before sampling is Vn-2, and the current position at that time is βn-2, then the current position of the traveling vehicle at time T I! n can be determined by the following formula.

In Figure 9, which graphically represents these four equations, time ('I゛-
The vehicle's current position ln at time T is obtained by adding the minute position change Δ shown in the shaded area in the figure to the current position It n-2 at 2.ΔL).

In the above four formulas, Vn, Vn-1, Vn-
2, i2n, An-1, l1n-2, and VnT as a velocity save area are written in advance in the area of the RAM 9-2.

Also, in those RAM areas, (Vn)-ΔL/3
・vn, (Vn-1)-Δt/3 ・vn-1, (
Vn-2)-Δt/3 ・vn-2, (Ln)=
ffn, (Ln-4) ='I2n-1, (Ln-
2) Each of -6n-2 is entered before execution of the quadratic approximation program.

A flowchart of such quadratic approximation is shown in FIG.

Therefore, the CPU 7 successively executes the programs of the quadratic approximation flowchart shown in FIG.

At the time of execution, the CPU 7 performs the addition and multiplication in equation (4) above using floating point operations using a subroutine in the ROM 9-1.

In this case, the subroutine is R-u
The following subroutines will be explained in order: (11) integer conversion subroutine, (iii) normalization subroutine, (1v) multiplication subroutine, and (v) addition subroutine.

(i) The floating point conversion subroutine converts a 2-hide integer into a 3-byte floating point number (2 bytes for the mantissa, 1 byte for the exponent)
used when converting to bytes).

That is, in order to convert a 2-byte integer to a 3-byte floating point number, the 2-hyde integer value is directly entered into the area of the 2-byte mantissa RAM 9-2. In this case, 1' of the exponent part? A
In the M area, enter 15 in hexadecimal and 0FH in decimal, and then normalize the 3 bytes of floating point number.

For example, if we set the 2-hyde integer in the bare D-E register to (H-
Floating point numbers + ((HL)), ((HL) + 1) in the 3-byte RAM area from the address pointed to by
)) into the mantissa part ((HL)-1-2 is the exponent part) and enter it.

(ii) The 1-defeat subroutine is used when converting a 3-hyde floating point number to a 2-byte integer.

When converting a 3-hyde floating point number to a 2-byte integer, pay attention to the decimal point position of the mantissa and make sure that the exponent B1+ is 1 in decimal.
If it is 5, the value of the mantissa becomes the integer 2 Hyde. If the exponent part is O or a negative number, the value α of the mantissa part is 0.5 (D)
≦1αl<1(D). This is because the floating point number is normalized in advance, so the mantissa Is high-order value is 0.1xxxx when positive and 1.0xxxx when negative. Therefore, the absolute value of a floating point number is less than 1, and when expressed as an integer, the decimal part is rounded down to 0. Also, the exponent part is 15' (D) (
OFH), when a floating point number is converted into an integer, it cannot be expressed as a 2-byte integer, resulting in an overflow.

The action taken at this time is to set all three outputs x, y, and z to zero and stop the CPU 7.

The flowchart of the integerization subroutine described above is shown in FIG.

(iii) The normalization subroutine is, for example, (H-L
) from the address pointed to by normalizing the l'Z moving and small moving points in the 3-high l-RAM area and using it as a program without returning to the original area.

As a result, many significant digits are taken from the first decimal place of the mantissa band of the floating point number, thereby reducing errors in the CPU's calculation results.

Specifically, when thinking in terms of binary 2's complement of 3-hyde floating, if the mantissa is positive, M f'fls≦' Normalized.

That is, when the mantissa part is 0, the floating mantissa part and exponent part are all set to zero.

When not normalized, 1 is subtracted from the exponent band each time the mantissa is shifted to the left by 1 bit. When shifting to the left, O is placed in the LSB of the mantissa so as not to change the value of the floating point number before normalization. Repeat this until normalized. During this repetition, 1 is subtracted from the exponent part each time the exponent part is shifted to the left by 1 bit, but if the value of the exponent part becomes smaller than the maximum negative number, it cannot be expressed with one bit of the exponent part. In this case, the floating mantissa and exponent parts are all zero. In reality, by repeating the left digit 15 times, numbers whose mantissa part is other than O are always normalized.

A normalization subroutine flowchart 1 as shown in FIG. 15 is shown in FIG.

(iv) The multiplication subroutine is executed from the address indicated by (HL) to 3 high [
Multiply the floating point number by the floating point number in the RAM area of 3 hides from the address pointed to by (H-B), and transfer the result to the RAM area of 3 hides from the address pointed to by (H-C). As a result, the floating point number in the 3-byte I-RAM area from the address pointed to by (H-L) is corrupted 1, and (H-B
) is a more unbreakable program.

Here, an example of multiplication of a 4-digit decimal floating point number will be given.

(0,2500X10')
2000X10)=(0,2500X0.20
00) X (10XIO)-〇, 0500 X
10 10, 5000xlO (normalization) As can be seen from this example, in general, when multiplying two floating point numbers, it is sufficient to perform each multiplication by separating the mantissa and exponent parts.

That is, when considering a binary floating point number, the mantissa part only needs to be shifted and added, and the exponent part is multiplied by 2x2, so in reality, the sum a+b of the exponents can be calculated.

Applying this to the multiplication routine -Fi ii
I will explain it in detail.

Here, we will multiply two 3-byte floating point numbers.
There is ζ.

First, in the case of mantissa multiplication, if the mantissa part of the multiplier or multiplicand is O, the result will necessarily be zero. Therefore, the mantissa and exponent parts of the result are all set to zero.

Here, an example of multiplication of two binary positive integers will be given.

The mantissa part of the score is also the same as in this example.

In other words, shift the mantissa part of the multiplier by 1 bin to the right, and L
If the carry from S I3 (hereinafter referred to as cy) is 1, the value of the mantissa area of the result and the value of the mantissa of the multiplicand are added.

The calculation result is stored in the mantissa area of the result. Then, the mantissa of the result is shifted to the right by 1 bit and the least significant digit is discarded. This is because the mantissa area of the result is limited to 2 bytes, so if you multiply the 2 bytes of the mantissa of the multiplicand by the 2 bytes of the mantissa of the multiplier, the mantissa of the result will be 4 hides, and the lower 2 hides will be 9JJ digits. This is because it becomes .

However, in the case of the present invention, the mantissa parts of the multiplier and the multiplicand are expressed as complements, and the MSB is a sign bit, and since both are positive, it becomes 0.

The result of the multiplication is 15 bits → -15 bits, which is 30 pintos, but the most significant bit of the 2-byte mantissa part of the result is used as the sign bit like the multiplier and multiplicand, so in the end,
The result of the multiplication is 30 bits -> -1 bit - 31 bits, and the number of invalid digits is also 15 bits. Therefore, the mantissa of the result only needs to be shifted to the right by 1 bit 15 times. In this case, the least significant digit is truncated for the first 14 times, but when the mantissa of the final result is shifted to the right, if cy from the LSB is 1, 1 is added to the mantissa of the result. In other words, the mantissa part of the result is rounded off at the final stage.

Also, the MSB of the mantissa part of the multiplier is the sign bit and is O (positive).
Therefore, it is only necessary to shift the mantissa of the multiplier by 1 bit to the right 15 times.

When the multiplier and multiplicand are negative, take the two's complement of the mantissa,
Calculate as a positive number. Then, only when either the multiplier or the multiplicand is negative, the two's complement of the mantissa part of the result is taken to make the result a negative number.

Next, regarding the multiplication of the exponent parts, when using this multiplication quantity broutine, the value of the exponent part of either the multiplier or the multiplicand is O or negative, so the multiplication of the exponent parts, i.e.,
It is thought that overflow will not occur when adding together the exponents.

However, in the addition of two negative numbers of 4-bit integers, the multiplier,
If both exponent parts of the multiplicand are negative, underflow may occur.

In this case, the determination of whether an underflow has occurred is as follows:
Both exponent parts of the number are negative, and the MS of the addition result of the exponents is
B. This can be done depending on whether the sign bit is O or not. If the result of the test 141i is an underflow, the exponent and mantissa parts of the result are all set to zero.

In other cases, it is sufficient to simply add one byte of the exponent part of the multiplication and one byte of the exponent part of the multiplicand and store it in the one-hide area of the result.

(■), the addition subroutine is executed at the address indicated by (HL) and (1-
Add the floating point numbers in each of the 3 high I areas from the address pointed to by 1-B), put them into the 3 high I-area from the address pointed to by (1-1-L), and put them into the RAM area from the address pointed to by (1-1-L). and(
Write a program in which the floating point number in the 3-hide RAM area from the address pointed to by H-B) has different values before and after calculation.

That is, when adding two floating point numbers, the exponent parts of the two numbers are compared, C1, and if the results are equal, the scale power is increased by +1j, so the mantissa parts are added and normalized.

If they are not equal, the lower exponent of the exponent part is adjusted to the higher exponent, and the mantissa of the number with the lower exponent is shifted to the right by the difference in exponents. Next, the mantissa parts of the shifted numbers are rounded off and the mantissa parts of the two numbers are added.

Here, we will discuss an example of addition of the top 4 hits of the mantissa part of two numbers. When a carry occurs beyond the decimal point of the mantissa part of the result, if the mantissa part of two numbers is If it is Ol (negative + negative), it is set to 1, and the mantissa of the result, including cy, is shifted to the right by one pit. At this time, c from LSB
If it is y, add 1 to the mantissa and round off here as well.

Then, by shifting to the right by 1 bit, 1 is added to the exponent part of the result.

If the mantissa part of two numbers is (positive → - negative), no carry occurs beyond the decimal point of the mantissa part of the addition result, so the mantissa part of two numbers is (positive ten positive) or (negative + negative). In the case of (positive 10 positive) and (negative → -negative), it is sufficient to process the case as if all carries beyond the decimal point occur and then normalize.

U, CI) U7 repeats the above-mentioned cycle and calculates the current position of the vehicle at the time of execution.

In this case, as is clear from the flowchart of FIG. 13, the CPU 7 calculates the distance traveled by the vehicle in each axis direction of x, y, and z in three-dimensional coordinates, and continuously outputs the results.

Here, to describe the output in detail, the output calculated in the program (current vehicle position at time 'r>
x, y, and z are expressed as 3-byte two's complement floating point numbers.

Therefore, subtract 7 from their exponents for X and Y, and subtract 3 from °ζ for Z. In other words,
Set the output values to X/2, Y/2, and Z/2 and adjust the output range by 1IFiI. If, IX/2 1, I
When Y/2 1 or +2/2 1 becomes less than 1, the 12-bit input to D/A converters 10-12 cannot be output because it is an integer. Therefore, in this case, the values of the three hide outputs X/2, Y/2, or Z/2 are all set to zero.

Then the 3 Hyde two's complement floating point number X/2Y/2
, Z/2 to a two-hyde two's complement integer.

Here, the input power to the D/A converters 10-12 must be a 12-bit offset binary integer.

Therefore, if the D/A converters 10 to 12 are operated in a bipolar manner, the converted 2's complement integer of 2 high 1- can be output only in the case shown in Table 5.

Table 5 Cases other than those in Table 5 cannot be expressed as 2'4a integers within 12 bits. Therefore, in this case ζyo Meno＼−
As a flow, D/A converters 10 to 12'
Strike CPtJ7 by setting all the horns X, Y, and Z to cello.
Then, it is possible to output (2 bits of 12 bits)
of? iR number integer is offset bina January 2nd throat G
To perform this conversion, bit 11 is inverted.

Here, 1 is expressed as the full scale of the analog outputs X, Y, and Z from the D/A converters 10 to 12.

(i) The sampling interval used for calculation in the program is ΔT (11) The sampling interval executed by interrupting the hardware part is ΔT (iii) The input of the vehicle running speed is scale -■m
When ax≦V<l/max, D/A converters 10 to 1
X, 'Y, and Z calculated from 2 have sampling intervals of Δ'F.

However, the actual sampling interval must be less than Δ'], and therefore x, y, and z are output at intervals of Δ1".

Therefore, to find the current vehicle position at time T, (73
, 69Km/h), ΔT=62. 5ms e c.

When Δt=125 mseS, the possible ranges of X, Y, and Z are approximately as follows.

-1° 310Km<X<1. 310Km-1,3L
OKm<Y<1.310Km-81,88m<
Z< 81.88m thus D/A converter 10
.about.12 successively input digital data values of the vehicle travel distances X, Y, and Z calculated as described above by C1FU, convert them into log data values, and transmit them to the output device 17.

The output device 17 in the illustrated example includes a two-dimensional graphic display (7-1) and a three-dimensional graphic display 17-2.

- The two-dimensional graphic display 17-1 is the D/A converter 10 to 12
By inputting the data values from , the two-dimensional traveling trajectory of the traveling vehicle (X-Y, Y-Z.

Z−X) to l’M’i<.

The three-dimensional graphic display 17-2 is connected to the D/A converters 10 to 12.
A three-dimensional traveling trajectory of the traveling vehicle is determined by inputting the data values resulting from the second CPU performing calculations based on the data values from the second CPU.

As described above, according to the present invention, when a vehicle is driven at a site, three types of data of the traveling vehicle, such as the vehicle speed, the road slope, and the traveling direction, are detected, and based on these data, the travel distance of the vehicle described in 1. By being able to continuously detect the current position of the vehicle, the three-dimensional travel trajectory of the vehicle can be accurately turned.

Therefore, it is possible to grasp the running route topography of work vehicles at various plants, tom construction sites, etc. in a short time.

In addition, we will promptly and smoothly carry out work specifications and vehicle model selection that match the road topography, appropriate assistance in vehicle operation, and improvements and changes, as well as enable safe unmanned vehicle operation. A variety of meridians) can be obtained.

[Brief explanation of drawings]

Fig. 1 is a block diagram of a running trajectory analysis system according to a preferred embodiment of the present invention, Fig. 2 is a coordinate representing the running azimuth angle of the vehicle, Fig. 3 is a coordinate representing the vehicle inclination angle, and Fig. 4 is a sin
A diagram showing the sin wave in the table, Figure 5 is the same S
The waveform diagram of inθ, Figure 6 is the waveform diagram of Cosθ, and Figure 7 is the waveform diagram of Cosθ.
For example, the waveform diagram for 5 inr, Figure 8 is the waveform diagram for Cosγ, Figure 9 is a diagram for explaining the quadratic system approximation, Figure 10 is a flowchart for calculating Sinθ, and Figure 11 is the waveform diagram for 5inT. Figure 12 is the flowchart 1 for calculating linear system approximation, Figure 13 is the flowchart 1 of the entire system program, Figure 14 is the flowchart of the integerization subroutine, Figure 15 16(A) and 16(B) are flowcharts of the multiplication subroutine.
)(B) is the flowchart 1 of the addition subroutine. 1 is a speed sensor, 2 is an azimuth angle sensor, 3 is an oblique angle sensor, 7 is a vehicle current position measuring means, and 17 is an output device. Figure 3 Figure 2 Figure 2 and Figure 4 1-/ Figure 6 osQ- Figure 7 Figure 8 Figure 14 C■E) Day 2, Name of the device Travel trajectory analysis system 3, Relationship with the person making the amendment Applicant: Caterpillar Mitsubishi Corporation 4, Agent @105 5-6-5-6 Shinbashi, Minato-ku, Tokyo Drawings subject to amendment Inner box Figure 16 (b), Figure 17 (b) 7, contents of amendment as per attached sheet.

Claims (1)

[Scope of Claims] (+) Equipped with vehicles such as construction machines, transport machines, etc.
C A system for analyzing the three-dimensional traveling trajectory of a heavy vehicle, which includes a sensor for detecting the traveling speed of the vehicle, a sensor for detecting the inclination angle of the vehicle with respect to the vertical axis, and a sensor for detecting the traveling direction of the vehicle. , the vehicle speed data, road gradient data based on vehicle inclination, and travel direction data detected by these sensors are sampled at regular intervals, and the distance traveled by the vehicle at the three-dimensional coordinates is calculated and measured from these sample values. a means for calculating the current position of the vehicle; and a means for calculating the current position of the vehicle based on the input data from the means.
A travel track analysis system comprising: an output device for drawing a track track. (2. Also in the system set forth in claim 1), the vehicle current position measuring means reads a data table corresponding to the sample data from each sensor from the memory and locates the vehicle in three-dimensional coordinates. Run! A system characterized by analyzing 1TIL traces. (3) In the system according to claim 1,
A travel 11i11 track analysis system characterized in that the output device consists of two types of devices that individually draw a two-dimensional travel trajectory and a three-dimensional travel 1iJt trace of the vehicle based on the output data value of the vehicle current position measuring means. .