Development of scanning systems and a three-coordinate manipulator for the installation of a milking robot

. In the course of the work, the authors developed a three-coordinate manipulator and a feedback system based on the udder profile scanner for the spatial orientation of the working body of the manipulator of the milking robotic installation. A significant advantage of the developed scanning system is the optimal design and acceptable accuracy of object detection. It was also demonstrated that the absolute error when moving the elements of the drive system of a three-coordinate manipulator does not exceed critical values and can be recommended for use in the design of a milking robotic installation.


Introduction
Nowadays, more and more industries are undergoing complex automation and robotization.In turn, in the Russian Federation, the automation of animal husbandry processes is developing much more slowly than others.Complex automation in dairy farming is provided by a very limited number of companies whose equipment is very expensive, including in operation.Idle time in the technological line of automated milking threatens the farmer with [1].
The main tasks on a dairy farm are: increasing the productivity of animals and reducing their incidence in order to increase the economic effect.To achieve this task, it is necessary to ensure the automation of technological processes at the cattle dairy farm and control a large number of parameters in different parts of the milk production process [2].
Digital dairy farm management solutions, including a system for managing the processes of milking and feeding in animal husbandry, can reduce production costs by at least 25%, and increase animal productivity by at least 15-20% [3][4].This, in turn, will lead to a positive economic effect for the farmer, to a more humane maintenance of animals, and will also increase the environmental friendliness of milk production.
One of the most promising directions in the development of agricultural automation is the construction of automatic milking systems -milking robots (MR) [5].
Industrial robots are widely used in manufacturing and in science.As a rule, industrial robots are represented by automatic programmable manipulators that perform complex operations of spatial moving [6][7][8][9][10].These robots are used in hazardous or high-precision manufacturing.They are used in light and heavy industry, and the list of robotic systems used in agriculture has been recently expanding.For example, large dairy farming companies are widely using milking robots, self-propelled robots for distributing and transporting feeds [11][12][13][14][15].The most technologically advanced solutions are milking robots, as they perform a number of operations, such as locating the teats, their pre-cleaning with brushes or washing in the milking cup, feeding, weighing an animal, one-by-one fitting teat cups over cow's teats, individual milking each quarter of the udder, assessing qualitative indicators of milk, and successive disengaging teat cups.This robotic system contributes to complete replacement of human labor and improved product quality, as well as ensures preventive protection against diseases resulting from dry milking [16][17][18][19].The most expensive and complex part of the milking robot is the system for manipulating the working units, and a feedback system for spatial orientation required for manipulations [20][21][22][23][24][25][26][27].This work is devoted to the development and testing of such systems for further use in a milking robotic installation.

Structure of the MR positioning system
One of the main components of the milking robot is its positioning system.The MR positioning system (shown in Figure 1) moves the manipulator in space.The movement of the manipulator in space is provided by the following equipment:  In the Z-axis: vertical rail with drive, gearbox and belt-driven carriage with two belts. In Y-axis: horizontal rail with drive, gearbox and carriage on belt drive with one belt. In the X-axis (turn Q): a drive with a gearbox providing rotation around the Z-axis.
Each axle has its own drive motor and gearbox.Each drive has two cables: a position sensor (encoder) and a power supply.Figure 1 shows the construction of the manipulator positioning system, the following designations are used: 1 -horizontal axis drive; 2vertical axis drive; 3 -rotation axis drive; 4 -movable carriage with fixed manipulator; 5horizontal axis rail; 6 -two vertical axis rail; 7 -rotation axis; 8 -manipulator; 9 -tools (milking cups and the place of installation of the udder profile scanner).Horizontal and vertical motors are connected to their axles by drive belts.The horizontal one has a single belt, and the vertical one has a double belt for safety.For safety reasons, the manipulator cannot be operated without both vertical belts.If it is necessary to replace the vertical belt, they should always be replaced by a pair.
A coupling with an elastic plastic insert is used to connect each belt drive shaft and the gearbox shaft.And a rigid coupling is used to connect the shaft of the rotating manipulator and the gearbox.
All belts in the manipulator positioning system are split belts, not a continuous loop.Each belt is fixed on the carriage, wraps around the drive pulley and the idle pulley, and then is fixed on the carriage again.The drive controllers monitor the execution of movement commands, as well as control the moment of resistance on the motor shaft.If there is a lot of resistance to movement during operation, the motor will limit torque for movement in the direction of the force to prevent damage to equipment (or injury to personnel/animal) and ensure safe operation.
3 Kinematics of the positioning system

Forward kinematics
In order to describe the position of the MR manipulator in the space, it is necessary to solve a direct kinematics problem, which boils down to determining the dependence that establishes the relationship between the absolute (coordinate system of the MR) and the associated coordinate systems (of each of the links).
Since various characteristic points of the manipulator act as the working tool in the MR positioning system, this must be taken into account in all kinematic algorithms.Thus, there will be as many different kinematic algorithms as the manipulator has characteristic points (coordinates of the working tool).The introduction of several characteristic points of the manipulator is made to simplify the rest of the algorithms of the MR, including when handling and setting control commands to the manipulator.In total, the following characteristic points of the manipulator can be distinguished:  The central point of the Udder Profile Scanner (UPS). The center point of the circle of the front right milking cup. The center point of the circle of the rear right milking cup. The center point of the circle of the rear left milking cup. The center point of the circle of the front left milking cup.
The mechanical structure of the positioning system of the MR's manipulator for the working tool of the front left milking cup is shown in Figure 2. The following designations are used: L 1 -moving the carriage of the MR's manipulator horizontally; L 2 -the length of the manipulator from the fixed point on the carriage to the bend of the working tool; L 3the length from the bend of the manipulator to the UPS glass (normal to the glass surface); L 4 , L 5 -the length from the surface of the UPS glass to the milking cups (rear and front, respectively); Q 1 -angle of rotation of the manipulator; Q 2 -bending angle of the manipulator; Q 3 , Q 4 -the angle of rotation to the milking cups (front and rear, respectively) relative to the normal of the UPS glass.
In Figure 2, time-varying parameters are shown in red, and constant values are shown in black.Based on the mechanical structure shown in Figure 2, we find the position of the working tool:  When solving the inverse kinematics problem for a given mechanical structure (Figure 2), it is possible to obtain several solutions that satisfy the desired position and orientation of the manipulator's working tool in the space.In this regard, it is necessary either to form constraints or to introduce additional optimization algorithms.The paper considers the option of introducing restrictions on permissible movement.

Inverse kinematics
The method of solving the inverse kinematics problem of the position for the MR's manipulator, in which variable parameters can be obtained in the form of an analytical dependence of the parameters of the kinematic scheme is called analytical.In this case, the process of finding variable parameters is reduced to calculating the values of previously obtained analytical dependencies.
To find a solution to the inverse kinematics problem by an analytical method for the kinematic scheme provided in Figure 2, we introduce the following substitution: Then we get:

Where
-the specified desired coordinates of the working tool of the MR's manipulator.
Equations ( 1) and ( 2) are used to obtain a solution to the inverse kinematics problem for determining the position of the working tool of the manipulator.

Moving along the Z axis
The vertical movement of the manipulator is carried out by rotating the drive (2) with a rotational speed n 2 .Using a belt drive (shaft diameter d 2 is 38 mm) and a gearbox with a gear ratio i 2 = 12.The linear velocity of movement of the manipulator is calculated by the following formula: So for the rotation of the drive 1000 rpm, the linear speed of the manipulator will be equal to 166 mm/s.

Moving along the Y axis
Horizontal movement of the manipulator is carried out by rotating the drive (1) with a rotational speed of n 1 .Using a belt drive (shaft diameter d 1 is 38 mm) and a gearbox with a gear ratio i 1 = 5.The linear velocity of the manipulator is calculated using the following formula: (5) So for the rotation of the drive 500 rpm, the linear speed of the manipulator will be equal to 199 mm/s.

Moving along the X-axis
Movement along the X axis is carried out by simultaneous rotation of the drive (3) and movement along the horizontal axis.The resulting velocity of movement along the X axis will consist of a linear velocity along the Y axis and a linear component of the angular velocity of rotation of the manipulator.The relationship between the speeds of rotation and movement along the Y axis is shown in Figure 3, where L 1 -moving the carriage of the manipulator along the horizontal rail, L 2 -the length of the manipulator from the fixed point fixed on the carriage to the working tool, Q 1 -angle of rotation of the manipulator.Find the velocity along the X axis: Thus we get the velocity along the X axis: (6) In order to ensure constant must compensate for Y-axis movement.Hence .

Rotation
The rotation of the manipulator is carried out by using a drive (3) with a rotational speed of n 3 and gearboxes with a gear ratio i 3 = 100.The rotation speed of the manipulator is calculated using the following formula: (7) So for 500 rpm drive rotation, the rotation speed of the manipulator will be equal to 31.4 rad/s.

Detection
An udder profile scanner (UPS) is used as positioning feedback.UPS is a device that discretely emits 2 laser beams per rotating block, on which 4 mirror prisms are fixed.For the entire cycle of work (full rotation of the block with prisms) we get 4 scanning lines, with a field of view or aperture angle y equal to 120°. Figure 4 is a schematic representation of a UPS with scan lines.As output, the system receives from UPS a point cloud, which is an x matrix, where = 4 is the number of rows equal to the number of lines; = 820 is the number of matrix columns or points in one line, calculated from the UPS aperture angle and resolving angle.
Knowing the initial received data and characteristics of the UPS, we calculate the coordinates of each point in Cartesian or polar coordinate systems, for the convenience of further processing, we recalculate the initial data into the coordinates of points in the Cartesian system according to the following expressions: (8) Where -is the normal distance to the current point; -is the resolving angle;point number.
The result of such a transformation is a point cloud, a graphical representation of one of the lines is shown in Figure 5.The DBSCAN data clustering algorithm was used to process the point cloud and detect objects.This data clustering algorithm is based on the density of data in a certain space, the algorithm groups points that are located together with each other and marks as outliers those points that are located in isolation.
Consider a set of points in some space requiring clustering.To perform DBSCAN clustering, points are divided into core points, point density reachable points, and outliers as follows:  Point p is the main point, if at least points are at a distance not exceeding , where -is the maximum neighborhood radius from to it (including point ), the same for the entire range and scan depth.These points are directly accessible from . A point is reachable along a line from , if point is at a distance not greater than from and must be the main point. A point is reachable from , if there is a path ,…, with , , where each point is reachable directly from (all points on the path must be primary, except for ).All points not reachable from the main points are considered outliers.The clustering algorithm is shown in Figure 6.In Figure 6 .Point A and the other red points are primary because the area with radius , surrounding these points contains at least 4 points (including the point itself).Since they are all reachable from each other, the points form one cluster.Points B and C are not basic, but reachable from A (via other basic points), and also belong to the cluster.Point N is a noise point, it is neither a main point nor directly accessible from point A.
DBSCAN iteratively goes through each point, and in the case of a controversial decision, if the points theoretically belong to several clusters, it can go through them several times.DBSCAN calls the method for determining the membership of neighboring points to determine whether points belong to a cluster.The algorithm performs one such method call for each point, and if an index structure is used that completes the neighborhood query in , we get the total average time complexity of , with a competent selection of the parameter , then in we get on average.The result of this algorithm is the output shown in Figure 7:

Description of the algorithm for finding the curvature of objects
The next step is the detection of cylindrical objects and the determination of their centers.This object iterates through the container holding the objects and calculates the radius of curvature and the center of each object.In each object, we take the first, middle and last points, two chords are drawn through these points (Figure 8).
The center of the circle is at the intersection of two perpendicular lines passing through the midpoints of the chords and .Since a line perpendicular to a line with a slope factor m has a slope factor of , then the equations of the lines perpendicular to and and passing through the midpoints and will be: (10) They intersect at the center and the solution gives: The value of is calculated by substituting into the equation of one of the perpendiculars.Having found all the curvature values of the objects, we delete those whose radius is outside the specified range.Figure 9 shows the result of the algorithm.

Test of the three-coordinate system
Tests of the three-coordinate system were carried out to verify the required accuracy of movement of the elements of the drive system and the working body of the manipulator.The tests were carried out as follows:  The current position of the movable mechanism along each axis was fixed on the structure. The drive system was moved a certain distance (absolute and relative displacement). The final movement of the drives along each axis was recorded and the error was calculated.

Test of the UPS
Tests of the udder profile scanner were carried out to verify the required accuracy of detection of cylindrical objects.The tests were carried out as follows:  The udder profile scanner was placed on scale paper, with an instrumental error of 0.5 mm. At distances of 100, 150, 200, 250, 300, 350 mm from the point of the beginning of the working range of the udder profile scanner, scanning objects were placed on the left and right areas for one line. The distance to these objects and the compliance with the declared detection error were determined by the software method.
The test results are recorded in the test report.

Data analysis and research results
Table 1 presents the results of a study of a three-coordinate system to ensure the required accuracy of movement of the elements of the drive system and the working body of the manipulator.2 and 3 present the results of the study of the UPS to ensure the required accuracy of detecting cylindrical objects.
The absolute error in moving the elements of the drive system was no more than 3 mm, which in relative terms does not exceed ±1%.The relative error of detecting cylindrical objects when using the udder profile scanner is no more than ±3.0% and is observed over the entire operating range of the scanner in depth (60-400 mm.).The test results

Figure 10
Figure10shows the appearance of the experimental setup.

Table 1 .
Results of the experiments of the three-coordinate system.

Table 2 .
Experimental results of the UPS for the left plane of the working range.

Table 3 .
Experimental results of the UPS for the right plane of the working range.