KR102222446B1 - Water spraying robot automated tracking system for dust suppression and method thereof - Google Patents

Abstract

According to one embodiment of the present invention, provided is a location tracking system of a water spray robot for removing fine dust, which includes a robot GPS which acquires location information of a water spray robot, and a water spray robot equipped with a motor which rotates to indicate the direction the water spray robot should face. The water spray robot receives the location information of an excavator from the excavator GPS installed in the surrounding excavator, calculates the relative location through the location of the excavator and the location of the water spray robot, calculates the driving angle of the water spray robot, and controls to point in the direction to be directed according to the location of the excavator, so that water spray is made to the excavator in the corresponding direction.

Description

TECHNICAL FIELD [Water spraying robot automated tracking system for dust suppression and method thereof]

The present invention relates to a location tracking system and method, and more particularly, to a location tracking system and method of a water spray robot for removing fine dust.

Currently, fine dust generated during building demolition work in Korea is suppressed through manpower sprinkling because there is no separate treatment technology.

However, there are many accidents in which workers are killed or injured due to a collapse accident during the workforce spying, so the workforce spying has a big problem that the safety of workers is not guaranteed.

Unlike the case of Korea, which treats fine dust through the conventional manpower sprinkling method, advanced foreign countries such as the United States, Japan, and Australia, including Europe, have developed manual water spraying devices and are using them in various industrial sites.

However, the conventional fine dust suppression system used overseas also requires a person to directly move the water spraying machine, and there arises a problem that the treatment efficiency of the fine dust decreases due to the inconsistency of the spraying direction of the excavator and the water spraying machine.

Therefore, in order to solve the above-described problem, as mentioned above, it is possible to solve the problem of disaster incidents and overseas technologies in Korea, and also research on a new fine dust suppression system technology to remove fine dust occurring in various industrial sites. Need

To this end, it is necessary to develop an automated fine dust tracking system focusing on the suppression of fine dust generated at construction sites among fine dust generated in various environments by utilizing technology that can recognize the location of the excavator and automatically track its position accurately. have.

(Non-Patent Document 001) Leung, KK, Liu, C., Wong, CC, Lo, JCY, and Ng, GCT, "On the Study of Ventilation and Pollutant Removal OverIdealized Two-Dimensional Urban Street Canyons," Building Simulation, Vol . 5, No. 4, 2012. (Non-Patent Document 002) Michioka, T., Sato, A., Takimoto, H., and Kanda, M., "Large-Eddy Simulation for the Mechanism of Pollutant Removal from a Two-Dimensional Street Canyon," Boundary-Layer Meteorology, Vol. 138, No. 2,

In the present invention, the water spray robot receives the location information of the excavator in the surrounding construction site, calculates the relative position, and provides a location tracking system and method of the water spray robot capable of tracking the location of the excavator based on a location tracking algorithm. I have a purpose.

A system for tracking a location of a water spraying robot for removing fine dust according to an embodiment of the present invention includes a robot GPS that acquires location information of the water spraying robot, and a motor that rotates to indicate a direction to which the water spraying robot should be directed. It includes a water spraying robot having a, wherein the water spraying robot receives the location information of the excavator from the excavator GPS installed in the surrounding excavator, and calculates a relative position through the location of the excavator and the location of the water spraying robot, By calculating the driving angle of the water spraying robot and controlling the motor to point in a direction to be directed according to the position of the excavator, water is sprayed to the excavator in the corresponding direction.

In the above, the excavator GPS is installed in the center of the excavator body, and the position of the excavator bucket portion is determined through Equations 1 and 2 below to determine the position and direction of the bucket from the origin of the installed excavator body after installation. Can be calculated.

[Equation 1]

Here, a _i (i = 1, 2, 3) is the length of each link, and Φ _i (i = 1, 2, 3) is x _i (i = 0, 1, 2, when the rotation is counterclockwise) 3) It _{refers to the angle between the z i} -1 (i = 1, 2, 3) and z _i (i = 1, 2, 3) axes with respect to the axis.

[Equation 2]

은 끝 점의 회전행렬(Rotation Matrix)을 표기한 것이고,

는 굴삭기 위치를 벡터로 나타낸 것을 의미함.here,

Is the rotation matrix of the end point,

Means that the excavator position is expressed as a vector.

In the above, the position of the excavator bucket portion is characterized by calculating through Equation 3 below using the rotation angle of the excavator body, the angle at which the boom portion is formed, and the angle at which the arm portion is inclined.

[Equation 3]

Here, (x*, y*, z*) means the position of the excavator bucket part.

In the above, the driving angle of the water spray robot is calculated by dividing the angle rotating in the horizontal direction and the angle rotating in the vertical direction, respectively, using Equations 4 and 5 below.

[Equation 4]

Here, α _yaw is the angle of rotation in the horizontal direction, l is the difference in latitude at the relative position between the water spraying robot and the excavator bucket, and y* represents the difference in hardness at the relative position between the water spraying robot and the excavator bucket.

[Equation 5]

Here, β _pitch is the angle of rotation in the vertical direction, and l is the difference in latitude at the relative position between the water spray robot and the excavator bucket part.

In the above, the rotation angle of the excavator body, the angle at which the boom part is formed, and the angle at which the arm part is inclined is calculated by installing an IMU sensor on the excavator body, boom, and arm, respectively, and using Equation 6 below. It is characterized.

[Equation 6]

Here, acc _i (i = x, y, z) means acceleration in the x, y, z axis directions obtained from the IMU sensor.

A method of tracking the location of a water spraying robot for removing fine dust according to an embodiment of the present invention includes location information of the excavator using the excavator GPS installed in the excavator after operating the excavator and water spraying using the robot GPS installed in the water spraying robot. Obtaining robot location information; Calculating a rotation angle of the excavator body, an angle in which the arm portion is inclined, and an angle in which the boom portion is inclined from an IMU sensor installed in the excavator; Using the excavator GPS installed in the center of the excavator body, the position of the bucket part corresponding to the distal part of the excavator is calculated through the Forward Kinematics Equation that can find out the position and direction of the distal part from the origin. step; Calculating and comparing the position of the bucket portion of the excavator and the relative position between the two of the water spraying robot using the calculated position value; Tracking the position of the excavator bucket by moving the water spray port of the water spray robot in a vertical direction and a horizontal direction using a motor of the water spray robot; And determining whether the direction of the water spraying robot coincides with the bucket portion of the excavator, and if the direction of the water spray robot coincides with the bucket portion, maintaining the current direction.

In the above, the step of tracking the position of the excavator bucket may include: when a horizontal error occurs due to a difference in horizontal position value during the comparison, the water spraying robot rotates in a horizontal direction to track the position; When a vertical direction error occurs due to a difference in the vertical position value during the comparison, the water spraying robot rotates in a vertical direction to track the position; It includes more.

In the above, when determining whether the direction of the water spraying robot coincides with the bucket portion of the excavator, when the direction of the water spraying robot does not coincide with the bucket portion of the excavator, the position of the bucket portion of the excavator and water spraying are performed using the recalculated position value. It further includes a step of calculating and comparing the relative position between the two of the robot.

The location tracking system and method of a water spray robot for removing fine dust of the present invention automatically performs the location tracking of the water spray robot to remove fine dust generated in various industrial sites, thereby reducing fine dust in industrial sites, There is an advantage of creating a pleasant working environment.

In addition, there is an advantage of being able to accurately track a direction in which water spray is required using a location tracking algorithm, and control the water spray to occur.

1 is a view for explaining the configuration of a position tracking system of a water spray robot for removing fine dust according to an embodiment of the present invention.
2 is a block diagram of a position tracking system of a water spray robot for removing fine dust according to an embodiment of the present invention.
3 is a view showing the setting of an excavator coordinate system for obtaining a position of a bucket portion of an excavator, which is an excavator according to an embodiment of the present invention.
4 is a view showing each variable for calculating a driving angle of the water spray robot according to an embodiment of the present invention.
5 is a flowchart of a method of tracking a location of a water spray robot for removing fine dust according to an embodiment of the present invention.
FIG. 6 is a diagram for explaining angle extraction using rotation angles θx, θy, and θz with respect to the x, y, and z axes of the IMU sensor of the present invention.
7 is a graph showing an experiment result of comparing an actual angle and an IMU sensor angle in consideration of the control environment of the acceleration sensor according to the present invention.
8 is a view showing a graph showing actual GPS coordinates of the excavators of Tables 2 and 3 of the present invention and experimental values using the excavator GPS sensor.
9 is a view for explaining the experimental results of whether the water injection robot of the present invention properly tracks the bucket portion of the excavator.
10A is a graph in which the location of a point is repeatedly measured using GPS.
10B is a diagram for comparing the actual measured location by inputting it into a map.
11A is a diagram showing an experimental environment of the present invention.
Figure 11b is a schematic diagram of the experimental environment of Figure 11a.

Hereinafter, specific embodiments of the present invention will be described in detail with reference to the drawings. However, the spirit of the present invention is not limited to the presented embodiments, and those skilled in the art who understand the spirit of the present invention can add, change, or delete other elements within the scope of the same idea. Other embodiments included within the scope of the inventive concept may be easily proposed, but this will also be said to be included within the scope of the inventive concept. In addition, components having the same function within the scope of the same idea shown in the drawings of each embodiment will be described using the same reference numerals.

1 is a diagram for explaining the configuration of a location tracking system of a water spray robot 100 for removing fine dust according to an embodiment of the present invention, and FIG. 2 is a diagram illustrating the removal of fine dust according to an embodiment of the present invention. It is a block diagram of a position tracking system of the water spraying robot 100 for.

In the location tracking system of the water spray robot 100 for removing fine dust of the present invention, the water spray robot 100 tracks the excavator that is the source of the fine dust at the surrounding construction site and sprays water, so that the dust can be suppressed. It is implemented as an automated system.

The location tracking system of the water spraying robot 100 for removing fine dust rotates to indicate the direction in which the water spraying robot 100 should be directed and the GPS 110 of the robot that acquires the location information of the water spraying robot 100 It includes a water spray robot 100 having a motor 120 to drive.

In particular, the water spray robot 100 receives the location information of the excavator from the excavator GPS 210 installed in the excavator, calculates the relative position between the two through the location of the excavator and the location of the water spray robot 100, and the By using a motor 120 to control the water spraying robot 100 to point in a direction to be directed, water is sprayed to the excavator in the corresponding direction. To this end, the water spraying robot 100 includes a robot GPS 110 and a motor 120, and an automatic control program for rotating and moving the motor 120 by calculating the location information of the excavator and a wireless communication function for transmitting location information. The robot controller 130 may be additionally included.

In an embodiment of the present invention, an automated system capable of suppressing dust by spraying water by tracking the bucket of the excavator 200, which is a major excavator that performs crushing and dismantling at a construction site that can generate a large amount of dust, is provided. About it.

Excavator GPS 210 (e.g., Beitian's BN-880 model) and robot GPS at the centers of the excavator 200 and the water spray robot 100, respectively, in order to find the location of the excavator 200 and the water spray robot 100 (110) is installed to obtain the absolute position (Absolute Positon) of each excavator 200 and the water spraying robot 100 based on GPS location information.

In order to obtain the end position of the excavator 200, a Forward Kinematics Equation using the location information of the excavator GPS 210 as an origin is used.

The rotation angle of the body of the excavator 200, the angle in which the arm is inclined, and the angle in which the boom is inclined are respectively an IMU (Inertial Measurement Unit) sensor installed on the body, arm, and boom (e.g., GY SENSOR's GY-86 model). ) To obtain.

Thereafter, the relative position between the two can be calculated through the calculated bucket position of the excavator 200 and the position of the water spraying robot 100, through which the motor 120 of the water spraying robot 100 (e.g., ROBOTIS's AX -12A Dynamixel) is used to indicate the direction that the water spraying robot 100 should face. Finally, the water spraying robot 100 tracks the position of the bucket of the excavator 200.

LabVIEW of National Instrument Co., Ltd. was used as software for implementing the excavator position tracking algorithm of the position tracking system of the water spray robot 100 for removing fine dust of the present invention.

3 is a view showing the setting of the coordinate system of the excavator 200 for obtaining the position of the bucket portion of the excavator 200, which is an excavator according to an embodiment of the present invention, and an excavator for obtaining (x*, y*, z*) It shows the coordinate system setting of (200).

The end portion of the excavator 200 used in the actual construction site uses a crusher, but in the example of the present invention, a bucket commonly used in the excavator 200 is used, and the location of the excavator 200 bucket is known. For this, it is easy to find out the location information of the corresponding part by installing a GPS (Global Positioning System) directly on the bucket.

However, the excavator 200 operating in an actual industrial site has a high possibility of direct contact with various impurities while contacting with building debris generated during demolition, including dust generated at the site, so that the GPS cannot be directly attached to the bucket part.

Therefore, in the present invention, the excavator GPS 210 is installed in the center of the body of the excavator 200, and then the position and direction of the end of the excavator 200 can be found from the origin of the installed excavator body. You can calculate the position of the bucket part.

At this time, the angle at which the excavator 200 rotates at the _{origin O 0 (θ 1} ), the angle at which the boom part is inclined (θ ₂ ), and the angle at which the arm part is inclined (θ ₃ ), θ _i (i = 1,2, 3) can be obtained through the IMU sensor 220 installed at each location.

Coordinate transformations from O _{0 to O 3} can be calculated through Equation 1, which is a homogeneous matrix that describes the relationship between two adjacent coordinate systems, and each calculated homogeneous transformation matrix is a bucket from the origin through Equation 2 The conversion to is calculated.

_{The location of O 0} , which is the origin of the excavator 200, can be known through the excavator GPS 210 attached to the excavator 200. In this case, a _i (i = 1, 2, 3) is the length of each link, and Φ _i (i = 1, 2, 3) is x _i (i = 0, 1, 2, 3) It _{means the angle between the z i} -1 (i = 1, 2, 3) and z _i (i = 1, 2, 3) axes with respect to the axis.

In addition, d _i (i = 1, 2, 3) is an offset length between the i (i = 1, 2, 3)-th joint of each link and is calculated as 0 in the present invention.

은 끝 점의 회전행렬(Rotation Matrix)을 표기한 것이고,

는 굴삭기(200) 위치를 벡터로 나타낸 것을 의미한다. The calculated total transformation can be expressed as Equation 2,

Is the rotation matrix of the end point,

Means that the position of the excavator 200 is represented by a vector.

는 수학식 3으로 표현되어 굴삭기(200)의 버킷 부분의 위치(x*, y*, z*)를 계산할 수 있다.

Is expressed by Equation 3 to calculate the position (x*, y*, z*) of the bucket portion of the excavator 200.

Here, θ ₁ is an angle at which the excavator 200 rotates, θ ₂ is an angle at which the boom portion is tilted, and θ ₃ is an angle at which the arm portion is tilted.

4 is a view showing each variable for calculating a driving angle of the water spray robot 100 according to an embodiment of the present invention.

The water injection robot 100 tracks the position of the excavator 200 bucket by moving the water injection port in the vertical and horizontal directions.

In order to track the position of the excavator, the position (x*, y*, z*) of the bucket portion of the excavator 200, which is an excavator, and the relative position of the water spray robot 100 are calculated.

The location information of the water spraying robot 100 can be measured through the mounted robot GPS 110, and the relative position of the bucket part of the water spraying robot 100 and the excavator 200 is l, which is the latitude difference, and y*, which is the longitude difference. Is expressed.

The calculated position difference value is substituted into Equations 4 and 5 calculated using an arctangent to calculate angles α _yaw and β _pitch that the water spray robot 100 should face.

α _yaw is the angle of rotation in the horizontal direction and β _pitch can be expressed as the angle of rotation in the vertical direction. The driving direction of the water spraying robot 100 is determined at the calculated angle, and after tracking the position of the bucket of the excavator 200, water is sprayed to suppress dust.

In addition, the rotation angle of the body of the excavator 200, the tilt angle of the boom, and the tilt of the arm using Equation 6, which is an angle conversion process from the IMU sensor 220 in the above-described position calculation process of the bucket part (Equation 3). You can calculate the true angle.

Here, acc _i (i = x, y, z) means acceleration in the x, y, z axis directions obtained from the IMU sensor 220.

In the present invention, as shown in FIG. 6, the angle is extracted using the rotation angles θx, θy, and θz of the IMU sensor 220 with respect to the x, y, and z axes.

In addition, in order to compensate for the characteristics of the acceleration sensor, which is somewhat vulnerable to vibration, a low pass filter that reduces generated noise is used.

5 is a flowchart of a method of tracking a location of a water spray robot for removing fine dust according to an embodiment of the present invention.

First, after operating the excavator 200, the location information of the excavator 200 using the excavator GPS 210 of the excavator 200 and the water spray robot 100 using the robot GPS 110 installed in the water spray robot 100 ) Can be obtained (S100, S102, S104).

Thereafter, the body of the excavator 200, the arm (arm), the body rotation angle of the excavator 200 from the IMU sensor 220 installed on the boom (boom), the angle in which the arm is inclined, and the angle in which the boom is inclined is calculated (S106). ).

In addition, in the center of the body of the excavator 200 in which the excavator GPS 210 is installed, the bucket part of the excavator 200 can be found through the Forward Kinematics Equation that can determine the position and direction of the end (end point) from the origin. The position is calculated (S108).

Thereafter, by using the calculated position value, the end position corresponding to the bucket portion of the excavator 200 and the relative position between the two of the water spray robot 100 are calculated and compared (S110).

Thereafter, the position of the bucket of the excavator 200 may be tracked by moving the water injection port of the water spraying robot 100 in a horizontal direction and a vertical direction using the motor 120 of the water spraying robot 100.

That is, when a horizontal direction error occurs due to a difference in the horizontal position value during the comparison, the water spraying robot 100 may rotate in the horizontal direction, and the vertical direction error due to the difference in the vertical position value during the comparison. When occurs, the water spray robot 100 may rotate in a vertical direction to adjust the position (S112, S114, S116, S118).

After the position adjustment, it is determined whether the direction of the water spraying robot 100 coincides with the end (end point) of the excavator 200 (S120).

If the direction of the water spraying robot 100 in step S120 does not coincide with the end of the excavator 200, the process returns to step S110 and the relative position may be calculated and compared again.

If they match in step S120, dust can be suppressed by spraying water while maintaining the current direction (S122).

An experiment was conducted to measure the error range of the IMU sensor 220 used in the present invention. 7 and Table 1 show the experimental results measured in consideration of the control environment of the acceleration sensor.

The actual angles to be measured were set to 0˚, 30˚, 45˚, and 60˚, and an experiment was conducted to see if the correct value was extracted by measuring this angle with the IMU sensor 220. The maximum error of the sensor value is 3.43˚, indicating that a relatively accurate angle is extracted.

The position of a point was repeatedly measured using the robot GPS 110 and the excavator GPS 210 provided in the present invention and shown in a graph as shown in FIG. 10A.

As a result of comparing the value of FIG. 10A with the actual measured location by inputting the value into the map as shown in FIG. 10B, it was confirmed that the difference between the actual location and the location measured by GPS is at least 7 m and up to 16 m. To reduce this error, the average of the GPS values repeatedly measured for the last minute was used.

At this time, the standard of one minute was set in consideration of the fact that the actual excavator 200 stays in one place for a long time and proceeds with the demolition work. In addition, as a result of taking the average value of the experiment, it can be seen that the difference between the actually measured position and the position of the average value of the experiment significantly decreased within 3 m as shown in FIG. 10A.

In the GPS-based location tracking experiment of the present invention, the water spray robot 100 can accurately track the excavator by increasing the accuracy of the GPS. Therefore, it is important to find out the exact position difference between the excavator 200 and the water spray robot 100.

Two calibrated robot GPS 110 and excavator GPS 210 were attached to each of the excavator 200 and the water spray robot 100, respectively, and the latitude and longitude between the two positions were measured. And based on this position, an experiment was conducted to calculate the relative position. In the experiment, the position of the excavator 200 was tested in the west 4 times and the east 4 times based on the water spray robot 100. At this time, the distance from the standard is 4.5 m.

Tables 2 and 3 show the calculated position value and the maximum error value when the excavator 200 was placed in the east and the west of the water spraying robot 100 and tested.

8 is a graph showing the experimental values in Tables 2 and 3. As a result of the experiment, the maximum positional error is measured to be about 1.2 m. Since the general water spraying robot 100 uses a water spraying nozzle that occupies a radius of 1 m or more, an error on a graph measured by an experiment can be said to be an error allowed to implement an actual system.

In order to test the accuracy of the location tracking system using GPS with improved accuracy, it was implemented similarly to the actual work environment. However, since it was difficult to use the actual excavator 200, a miniature model excavator 200 with a boom length of 47 cm and an arm length of 20 cm was used in the experiment. A laser pointer that can point to a point is mounted so that you can check the position tracking. The latitude difference of the water spray robot 100 was set to 4.5 m, and the longitude difference was set to 0 m. Fig. 11A shows the experimental environment implemented, and Fig. 11B is a schematic diagram of the experiment environment. During the experiment, it was assumed that the location of the excavator GPS 210 was fixed so that the excavator 200 stopped in one place and operated while driving the boom and the arm.

After setting the experimental environment, an experiment was conducted on whether the water spraying robot 100 properly tracks the bucket portion of the excavator 200 while the arm of the excavator 200 is moved to operate the arm similar to the actual work. One point on the bucket of the excavator 200 _{moved along the same path as P i} (i = 1, 2, 3) on _{the y-z graph of FIG. 9, and as the position of P i} changes, the water spraying robot 100 The laser pointer installed _{in indicated the position of P i} '(i = 1, 2, 3) in FIG. 9.

Of Fig for positioning accuracy observation of the water jet robot (100) 9 P _i and point _{P i '(i = 1,} 2, 3) by comparing the coordinates of the center position it is shown in Table 4. The maximum error of the y value is 0.025 m and the maximum error of the z value is 0.035 m, which shows the result that it is smaller than 0.04 m, which is a quarter length of 0.16 m, which is the bucket length of the excavator 200 used in the present invention.

This means that the location of the water sprayed by the water spraying robot 100 is stably included in the location range of the bucket of the excavator 200 where dust is generated. Therefore, it can be seen that the water spraying robot 100 is tracking the exact position whenever the bucket part of the excavator 200 moves, and even when the fine dust suppression operation is performed using the actual sized excavator 200 in the future, It is expected to show traces.

100: water spraying robot 110: robot GPS
120: motor 130: robot controller
200: excavator 210: excavator GPS
220: IMU sensor

Claims (8)

delete delete delete delete delete After operating the excavator, acquiring location information of the excavator using the excavator GPS installed in the excavator and the location information of the water spraying robot using the robot GPS installed in the water spraying robot;
Calculating a rotation angle of the excavator body, an angle in which the arm portion is inclined, and an angle in which the boom portion is inclined from the IMU sensor installed in the excavator;
Using the excavator GPS installed in the center of the excavator body, the position of the bucket part corresponding to the distal part of the excavator is calculated through the Forward Kinematics Equation that can find out the position and direction of the distal part from the origin. step;
Calculating and comparing the position of the bucket portion of the excavator and the relative position between the two of the water spray robot using the calculated position value;
Tracking the position of the excavator bucket by moving the water spray port of the water spray robot in a vertical direction and a horizontal direction using a motor of the water spray robot;
Determining whether the direction of the water spraying robot coincides with the bucket portion of the excavator, and if the direction of the water spray robot coincides with the bucket portion of the excavator, maintaining the current direction;
When determining whether the direction of the water spraying robot coincides with the bucket portion of the excavator, if the direction of the water spraying robot does not coincide with the bucket portion of the excavator, the position of the bucket portion of the excavator and the two of the water spraying robot are used using the recalculated position value. Including the step of calculating and comparing the relative position,
Tracking the position of the excavator bucket,
In the comparing step, when a horizontal direction error occurs due to a difference in the horizontal position value, the water spraying robot rotates in the horizontal direction to track the position;
In the comparing step, when a vertical direction error occurs due to a difference in the vertical position value, the water spraying robot rotates in a vertical direction to track the position; further comprising,
The IMU sensor uses a low pass filter to reduce generated noise to compensate for the acceleration characteristics of the IMU sensor, which is somewhat vulnerable to vibration,
The water spray robot calculates a driving angle of the water spray robot using the calculated relative position, and controls the water spray robot to point in the direction to be directed using the motor according to the calculated driving angle. Water injection is made by tracking the position of the bucket of the excavator in the direction,
The excavator GPS is installed in the center of the excavator body,
The position of the excavator bucket part can be calculated through Equation 1 and Equation 2 below, which can find out the position and direction of the bucket from the origin of the installed excavator body after installation,
The position of the excavator bucket part is
Using the rotation angle of the excavator body, the angle at which the boom part is formed, and the angle at which the arm part is inclined, it is calculated through Equation 3 below,
The driving angle of the water spraying robot is
It is calculated by dividing the angle rotating in the horizontal direction and the angle rotating in the vertical direction, respectively, using Equations 4 and 5 below,
The rotation angle of the excavator body, the angle at which the boom part is formed, and the angle at which the arm part is inclined is
IMU sensors are installed on the excavator body, boom and arm,
A method of tracking the location of a water spray robot for removing fine dust, characterized in that it is calculated using Equation 6 below.
[Equation 1]

Here, a _i (i = 1, 2, 3) is the length of each link, and Φ _i (i = 1, 2, 3) is x _i (i = 0, 1, 2, when the rotation is counterclockwise) 3) It _{refers to the angle between the z i} -1 (i = 1, 2, 3) and z _i (i = 1, 2, 3) axes with respect to the axis.
[Equation 2]

here,

Is the rotation matrix of the end point,

Means that the excavator position is expressed as a vector.
[Equation 3]

Here, (x*, y*, z*) means the position of the excavator bucket part.
[Equation 4]

Here, α _yaw is the angle of rotation in the horizontal direction, l is the difference in latitude at the relative position between the water spraying robot and the excavator bucket, and y* represents the difference in hardness at the relative position between the water spraying robot and the excavator bucket.
[Equation 5]

Here, β _pitch is the angle of rotation in the vertical direction, and l is the difference in latitude at the relative position between the water spray robot and the excavator bucket part.
[Equation 6]

Here, acc _i (i = x, y, z) means acceleration in the x, y, z axis directions obtained from the IMU sensor. delete delete

Images