A 3D model based control of an excavator.doc

A 3D model based control of an excavatorTomi Makkonen a,*, Kelervo Nevala b,a, Rauno Heikkila ca University of Oulu, Department of Mechanical Engineering, P.O. Box 4200, FIN-90014, Finlandb VTT Electronics, P.O. Box 1100, FIN-90570 Oulu, Finlandc University of Oulu, Research Unit of Construction Technology, P.O. Box 4400, FIN_90014, FinlandAccepted 26 July 2005AbstractNew technologies facilitate machine control systems, which use data from CAD and 3D measurement systems to automatically control construction machinery, such as an excavator. This paper examines the possibilities of controlling a six degrees of freedom (DOF) excavator with the final objective of controlling the movements of the excavator by using a positioning system such as a GPS in conjunction with a CAD model of the road surface. In comparison with the traditional 4 DOF machine, the excavator was provided with two additional degrees of freedom by applying a 2 DOF Rototilt, an accessory commonly in use. To study this problem, Msc.Adams with Matlab/Simulink was used as a simulation environment. In this paper the path of the bucket is calculated by using the CAD reference of the target plane. D 2005 Elsevier B.V. All rights reserved.Keywords: Robotic excavator; Path generation; Dig planning; Modelling; Simulation; Triangular terrain model; CAD1. IntroductionA robotic excavator is not a new idea as there are many interesting and advanced systems; for example, Autonomous Truck Loading from Carnegie Mellon University 1, LUCIE from Lancaster University 2 and Komatsu PC05-7 in ACRF 3. However, in general, the level of automation in the construction industry is relatively low compared to that in the other fields of industry. Although various research projects have been conducted for automating different tasks in construction, only a few applications are commonly in use. It would seem, however, that the general level of technology has finally reached a point where there are means of meeting even the specific needs of construction automation. In our study we have focused on, firstly, the intensive use of a modelling environment, secondly, an assumption that a CAD model of the target surface is available, thirdly, a 3D positioning systems such as GPS is available, and fourthly, the excavator has six degrees of freedom instead of the traditional four degrees.应验Simulation tools like Msc.Adams and Matlab/Simulink are well suited for virtual development of excavator automation ,especially so for considering the problem of path generation or checking a set of equations like inverse kinematics. They are an efficient tool when developing new control methods andalgorithms. Properties of an automated system can be designed in detail before the new methods move to the prototype stage .Also, development work will be accelerated and solutionsoptimized accordingly.A road surface CAD model with 3D measurements has been used successfully in machine control automation, at least for the road grader 4,5. The reported speed increase was between30% and 60%.Based on the triangular terrain model, the theme in this paper is to derive equations for the buckets position and orientation so that paths L1, L2 and L3 are obtained (see Fig. 5). We are also interested in defining the kind of information needed about the CAD model and to present it accordingly.1.1. Excavator with 6 DOFT. Makkonen et al. / Automation in Construction 15 (2006) 571 577Fig. 1. Six coordinate systems attached to an excavator with Rototilt.the orientation of a buckets cutting edge which is always parallel to the y-axis of the coordinate system K0 (see Fig. 1),thus limiting the accuracy of bucket control.So, even if the centre point of the buckets cutting edge is at the desired location, its corner might be displaced several tenths of a centimetre from the target surface. If the yaw angle of an excavator is 45-, the displacement of the corner of a 1-mwidebucket can be as high as 35 cm.Rototilt 6 is shown in Fig. 2. It is widely used (and well-liked) among excavator operators because it generates two additional degrees of freedom yielding the driver more power to operate. Moreover, the necessity of moving the excavator body is greatly reduced while, for example, digging trenches with a 4 DOF excavator is only possible when the body is located in the same line with the trench. With 6 DOF, this can also be done from the side of the trench line.1.2. Short introduction to a 6 DOF excavator model and previous workGeometry of an excavator is drawn using Msc.Adams 7, a program designed to simulate mechanical systems. For control and programming the Matlab/ Simulink environment is used .Both products are linked so that the model inside Msc.Adams can be controlled from Simulink, which speeds up development work.A basic model includes:1. Analytically solved inverse kinematics for a 6 DOFexcavator (Fig. 1).2. Positioning and orientation using 2 GPS sensors and aninclinometer (Fig. 3) so that the transformation matrix TMCSSCS between the SCS (Site Coordinate System) and MCS(Machine Coordinate System) can be calculated.Fig. 2. Rototilt and demonstration of possibilities with extra two degrees of freedom.3. A velocity P-controller to drive the joints to a position calculated by the inverse kinematics.Some information on the simulation of a 6 DOF excavator can be found in 8.2. Path generation for 3DThe file format we chose for the study of the excavator automation is the triangular terrain model (see Figs. 4 and 8). Triangles defined by vertices determine the plane. It is possible to generate a text file of the x, y, and z coordinates of the points defining the vertices of the triangles. This xyz text file is the starting point for our study.2.1. Triangle compilationAs mentioned above, the xyz format is the base of calculations. The first step is to transform the information into triangles as information given as points alone is not enough for the purposes of path generation. The Delaunay triangulation method 9 was chosen as the method of triangulation because it is included in the Mat lab function library. After triangulation we get a matrix in which every row is composed of three points in space and a triangle number. The triangle number is called Tri.After joining the triangles together, the unit normal vector w is calculated by using a cross product for every triangle. If we denote the vertices as N1(x1, y1, z1), N2(x2, y2, z2) and N3(x3,y3, z3), and 2 sides of a triangle as a and b we get:Therefore, the unit normal for the triangle formed by the vertices N1, N2, and N3 can be calculated as follows: There are two possible directions for unit normal vector w and only one is chosen so that its direction in the SCS coordinate system (see Fig. 3) is in the positive z direction This determination is done in an IFTHENELSE statement as follows:When considering a large model of a highway with a large number of triangles it is practical to bind sorting parameters for every triangle. A simple solution is to calculate the maximum and minimum distance between the triangle and the SCS coordinate frames origin as follows:When the position of a point is known in the SCS frame, only the triangles that fulfil the condition a point can be in a triangle only if the distance of the point is between distmin, distmax are considered.The resulting triangle compilation matrix size is Tri_14 and the form is then as follows:2.2. Basic math for triangles, lines and pointsSurprisingly, only simple mathematics is required when working with the L13 path generation problem in Fig. 5. Two basic problems can be defined:At what point p does a line intersect a plane defined by triangle vertices? This problem can be solved as in 10 as follows:Any of the three vertices can be chosen as the point N. If w IN =0, the line is parallel to the plane.Fig. 4. Part of road in a triangular format. Picture captured from MicroStation.Fig. 5. Schematic representation of a top view of an excavator on a triangular surface. L1, L2 and L3 are target paths for the bucket2) If a point is on the plane defined by the triangle, is it located inside the area defined by the vertices? Fig. 6 presents a situation where point p (x, y, z) is located inside the triangle, defined by points N1, N2 and N3. (In a simulation environment, it is practical to give two local origins which are located at points N1 and N2.) Fig. 6 shows that point p can be defined as a component of vectors a, b and d as follows:where the coefficients, t1, t2, t3, and t4 must be positive in order for the point to be inside the triangle. (Since the point is on the plane of the triangle, we can discard the z-coordinates.) Solving Eq. (7) yields:And if every ti0, the point is located inside the area defined by the vertices.2.3. Identification of triangles on the path in 3D The next step is to find triangles which are on the path of vectors L1, L2 and L3 (see Fig. 5). To simplify the calculation, we can remove the z-coordinate information as the projection to the xy-plane includes all necessary geometric information.Let us denote the end point of L1 as L1end x, y, z and we choose the direction of L1 as follows:This is not perfectly accurate, but if the excavator is close to the target plane and the reach is approximated as a sphere, it gives a close enough approximation for an initial study.For the L1end we then get:where OMCS1.2 is the MCS origin in the SCS frame, and distmaxL1 is a system-specific parameter giving the maximum reach of an excavator, similarly distminL1 is defined as the minimum reach. For the complete vector L1 we can then write a discrete function:where n =1, 2, 3. . . kmax and kmax is the number of samples and gives a resolution for triangle change. When n =kmax, the starting point of L1 is acquired.The procedure is the same for L2 and L3, only L2endx, y and L3endx, y are calculated slightly differently by using parameter klev which defines the distance between paths L1, L2 and L1, L3. It should be noted that klev is given in 2D, and thus in 3D the distance might vary.The next step is to find triangles by using Eq. (8) and then reduce all unnecessary data points. An example of the resulting group is given in Fig. 7. One can see how the points are chosen at the end and the start of the path, and between them only when the triangle changes.After specifying the path matrix in 2D it is transformed into 3D by using the normal vector w, which was created at the triangle compilation stage. One form of the equation for the plane is:A data set (the same as in Fig. 7) for the path point matrix in 3D is illustrated in Fig. 8.2.4. Path and orientation of a bucketIn a previous study of a 6 DOF excavator, the inverse kinematics was created to bind the K0 and K6 coordinate systems (see Fig. 1), and the bucket was left out of the study. The reason for this was mainly that the bucket shape and size might vary a lot, and that it was still unclear what the best position andorientation for K7 (see Fig. 9) would be. So, we need to create a transformation matrix from the bucket tip to K6 as follows:To simplify the creation of path vectors, a control matrix is defined by using points created in Section 2.3. The format is:As one can see, the first column contains the triangle number, so we can use the tricom matrix (5) to read specificFig. 7. Points defining L1, L2 and L3 on a 2D triangulated surface. Compare toFig. 5.Fig. 8. Points defining L1, L2 and L3 on 3D triangle surface. Data set is the same as in Fig. 7.information from triangles. The last three columns define the points illustrated in Fig. 8. The path is then defined as follows:The above parameters can be expressed as a function of time so that d =speed time. In this way the constant speed for bucket movements is achieved. For index i, a triangle change, we get a simple condition:Orientation of a bucket is calculated when buckets xy plane is parallel to the triangle it is travelling over. Let us name the transformation matrix, which defines orientation and position of a bucket in the SCS frame as T7 SCS. By examining Fig. 9, it is clear that the z-axis of the K7 coordinate systemFig. 9. Coordinate systems K6 and K7.Fig. 10. Superimposed picture of an excavator performing cuts L1 3.has to be in the opposite direction of the triangle normalvector w. Thus we get:Logically (see Fig. 9) we can see that the direction of the yaxis should point away from the excavator. More exactly its direction should be opposite to pathvec:We know that the x-axis needs to be on the plane defined by the triangle (as is the y-axis) and its direction is obtained using the cross product as follows:The transformation matrix T7 SCS now fully defines the desired bucket position and orientation in the SCS frame.2.5. Verifying the equations for path generation in a simulation environmentA target surface similar to Fig. 8 representing a flat area and a slope was created in Msc.Adams. The idea is to simulate the following sequence which was considered effective for a road construction project:1. When an excavator is brought to the site, tricom matrix (5) containing information about triangles is calculated as presented in Section 2.1.2. The driver decides where to locate the excavator and when to start robotic excavation. 3. The transformation matrix TMCS SCS is calculated based on GPS information.4. By using equations presented in this paper, pathcont matrix (16) is calculated using the path set points for L1, L2 and L3. Fig. 10. Superimposed picture of an excavator performing cuts L1 3.Fig.Fig. 11. Simulated excavator performing cuts L1, L2 and L3. Position and velocity of the centre point of a cutting edge. Buckets target speed=1000 mm/s.5. As the excavator moves the transformation matrix is constantly updated. The orientation and position presented in matrix change for every triangle change and for every simulation step.6. Using matrix to transform into the Machine Coordinate System, the desired set points and orientation for the bucket can be calculated. Then by using the inverse kinematics equations joint angles for the excavator are calculated.7. As a result, the excavator will follow desired paths. A superimposed picture of an excavator performing the sequence is presented in Fig. 10. However, since the target of the simulation was to verify the equations presented above and the logical correctness of information flow, the simulation is not perfect as can be seen in Fig. 11. The path between vectors for example, from L1 end point to L2 start point is not controlled by the path vector which leads to high movement speeds. Also it should be noted that the cutting edge does not act smoothly when the bucket goes over the corner line (Fig. 10). Namely the set points for joint angles are calculated directly for an individual joint through inverse kinematics without any coordination between the joints. Because of this, when the corner line is crossed, the new group of calculated set points for the joints is very different from the previous one, and the individual joints will attain their newly calculated position at different times.Another source of error, and also a limitation on the way point calculation presented in this paper, is the requirement for constant bucket speed (17). When travelling over a sharp turning point, like the corner line in Fig. 10, the buckets target speed should be lowered. We suggest that variation in speed be included in path calculation in the future studies.3. ConclusionThis paper showed how to use a CAD model of a surface to position an excavator the bucket. A triangular terrain model was first compiled in the xyz file format, and then equations were derived from that file in order to calculate the target transformation matrix used to control the position and orientation of the excavator bucket.By combining this result with the transformation matrix , which in our model was created using information from 2 GPS sensors and an inclinometer, it was possible to form the way points for the bucket in 3D, and for a 6 DOF excavator it was also possible to define the orientation. Matrices defining the target points, pathcont, and information gathered from triangles, tricom, were found to contain all the data needed to create the path.Equations and data flow were modelled in parallel using the Msc.Adams and Matlab/Simulink programs. A 6 DOF excavator performing three cutting lines (Fig. 5), using a CAD model of the terrain and a 3D positioning system was shown in Fig. 10. The formulas presented above provide a basic tool for working with a CAD model of the target surface, and especially for controlling an excavator. The simple theme of finding the xyz points at which a straight line projects itself onto a 3D surface and of reducing the information to a minimum by using the table of triangles, should be useful in more complex cases than this one. A most ambitious idea is to automate the movements o