[0038] In order to facilitate the understanding of those skilled in the art, the present invention will be further described below in conjunction with the embodiments and the drawings, and the content mentioned in the embodiments does not limit the present invention. See Figure 1 to Figure 6 In the following, the present invention will be described in detail with reference to the accompanying drawings.
[0039] Provided by the invention One A SCARA manipulator control method based on a motion controller includes a motion controller and an industrial touch screen. The touch screen has secondary development functions such as position display, teaching, route planning, general IO input and output. The motion controller communicates with the touch screen through the Ethernet communication port, and the control method steps are as follows:
[0040] Step 1: The user plans the motion trajectory of the SCARA manipulator and its corresponding motion parameters through the touch screen, and saves the motion trajectory and parameters in the motion controller; preferably, the motion trajectory in the step 1 includes point to point, Linear interpolation and circular interpolation motion track. The SCARA manipulator of the present invention includes two rotating arms, an execution shaft is arranged on the second rotating arm, and a total of four motor shafts are provided.
[0041] Such as Figure 4 Shown is a movement trajectory case of the present invention; in the first line, the specified point-to-point speed is 50% of the maximum speed; in the second line, the set point-to-point acceleration and deceleration time are both 300ms; the third line, Specify the speed of linear interpolation and circular interpolation as 2000mm/s; in the fourth line, set the acceleration and deceleration time of linear interpolation and circular interpolation are both 500ms; in the fifth line, plan the SCARA manipulator to move from point to point (0,300,0,0), and the SCARA manipulator is right-handed when it moves to the target point; the sixth line, the SCARA manipulator moves from the current position (0,300,0,0) to the target position (300,300,0) in a straight line ,0), and the SCARA manipulator maintains the posture of the starting point during the movement, that is, the right hand posture; the seventh line, the SCARA manipulator draws an arc from the current position (300,300,0,0) clockwise to ( 300,300,0,0), the arc radius R is 50mm, that is, a clockwise circle with a radius of 50mm is drawn, and the manipulator maintains the posture of the starting point during the movement, that is, the right-hand posture.
[0042] Step 2: The motion controller calculates the actual motion trajectory of the SCARA manipulator according to the algorithm corresponding to the trajectory planning method, simulates its motion trajectory in the Cartesian coordinate system established by the virtual axis, and obtains it from the virtual axis Cartesian coordinates , And then set the rectangular coordinates of the virtual axis Converted to the rotating coordinates of the SCARA robot.
[0043] Further, when the motion trajectory is a point-to-point motion, the parameters include the coordinates of the end point, the rotation speed, and the posture of the SCARA manipulator; when the motion trajectory is a linear interpolation motion, the parameters include the end point Coordinates and interpolation speed; when the motion trajectory is circular arc interpolation motion, the parameters include end point coordinates, circle center coordinates or arc radius or points on the arc and arc interpolation speed.
[0044] Still further, the steps of converting the rectangular coordinates of the virtual axis into rotating coordinates in the step 2 are as follows: Step 2.1: When the motion trajectory is linear interpolation, in the virtual coordinate system, according to the specified acceleration, deceleration and speed , Start the virtual axis to move from the current position to the end position by linear interpolation, and then perform step 2.3;
[0045] Step 2.2: When the motion trajectory is circular interpolation, in the virtual coordinate system, start the virtual axis in circular interpolation according to the specified acceleration, deceleration, speed, and center or radius or points on the arc Move from the current position to the end position, and then perform step 2.3;
[0046] Step 2.3: In the rectangular coordinate system established by the virtual axis, move the virtual axis to the end point coordinates according to the motion trajectory and parameters given in step 2.1 or step 2.2; at the same time, turn on each scan cycle to obtain the position of the virtual axis, and you will get Virtual axis position Converted into the rotation position of each axis of the SCARA robot.
[0047] The specific conversion method in step 2.3 is as follows:
[0048] Step 2.3.1: Change Convert to ,
[0049] Suppose the arm lengths of the first arm and the second arm of the SCARA manipulator are A and B respectively, then the side length of the hypotenuse C is ,
[0050] According to the law of cosines
[0051] ,
[0052] ;
[0053] Step 2.3.2: Determine the quadrant of the target point
[0054] when , The target point is in the first and second quadrants, and the angle between the hypotenuse and the coordinate system is;
[0055] when , The target point is in the fourth quadrant, and the angle between the hypotenuse and the coordinate system is;
[0056] when , The target point is in the third quadrant, and the angle between the hypotenuse and the coordinate system is;
[0057] Step 2.3.3: When the SCARA manipulator is in the right-handed posture, ,;
[0058] When the SCARA manipulator is in the left-handed posture, ,;
[0059] Step 2.3.4: For u-axis, when rectangular coordinates are used as the reference system,;
[0060] When the z-axis is a screw structure, no need to change,.
[0061] In this embodiment, the linear interpolation motion track is taken as an example; here is an example Figure 4 The linear interpolation motion trajectory of the fifth line in the middle; this motion trajectory only has Axis movement, The axis is not moving, such as Figure 5 As shown, the SCARA manipulator moves from P1 to P2. When starting the manipulator, first start the virtual axis , Make it move in a straight line from the current position to the target position according to the specified speed and acceleration/deceleration time in the established rectangular coordinate system. In this embodiment, the The axis will follow Image 6 The speed and position shown move linearly from point P1 to point P2.
[0062] At the same time, the motion controller obtains each scan cycle (such as 1ms) The position of the axis in Figure 5 In the figure below, two of the moments versus Read Axis position versus; Combine get versus; Substitute it into Get the side length:
[0063] ,
[0064] Assuming that the arm lengths of the first arm and the second arm of the SCARA manipulator are both 300mm, substitute as well as get
[0065] ,
[0066] ,
[0067] due to , The target point is in the first and second quadrants, and the angle between the hypotenuse and the coordinate system is , which is
[0068] ,
[0069] Since SCARA is in a right-handed posture, , which is
[0070] ,
[0071] ,.
[0072] Step 3: Get the rotating coordinates of the SCARA manipulator Position, and the current position of each motor shaft of the manipulator Compare the difference , And calculate the position and rotation speed of each motor shaft in the next cycle according to the difference; here the current position of each motor shaft of the manipulator And the position of the previous cycle the same. From step 2 we can know the rotation angle and speed of the two motor shafts of the SCARA manipulator: the rotation angle of the first motor shaft is , The rotation speed is; The rotation angle of the second motor shaft is , The rotation speed is; The rotation angle of the third motor shaft is 0, and the rotation speed is 0; the rotation angle of the second motor shaft is 0, and the rotation speed is 0; It is the scan period.
[0073] Step 4: The motion controller adopts the electronic cam control method, and assigns the position and speed obtained in step 3 to the electronic cam parameters, and rotates to the target position at a given speed to obtain the actual motion trajectory information of the SCARA manipulator, and then The actual motion trajectory information is sent to the SCARA manipulator. Directly add the position obtained in step 3 , And speed , As a parameter of the electronic cam control mode, the motor shaft will move from the current position to the , The position of the manipulator can be realized from the Pi point in the virtual axis to the Pi+1 point, and the distance from Pi to Pi+1 is small enough, the trajectory of the end of the manipulator swing from Pi→Pi+1 can be regarded as a straight line. The P1→P2 straight line is composed of many short straight lines from Pi→Pi+1, so as to realize the linear movement of the manipulator from P1→P2.
[0074] In summary, the present invention One A SCARA manipulator control method based on a motion controller, which directly compiles and plans the motion trajectory of the SCARA manipulator and its corresponding motion parameters through the touch screen. The user program can be written using the touch screen and the working position is given; the virtual axis is used to construct a virtual The rectangular coordinate system, using the interpolation algorithm inside the motion controller, can greatly reduce the amount of calculation, effectively simplify the motion algorithm, reduce the development difficulty of SCARA control system technology, shorten the development cycle, and lower the cost.
[0075] The above content is only the preferred embodiments of the present invention. For those of ordinary skill in the art, according to the ideas of the present invention, there will be changes in the specific implementation and the scope of application. The content of this specification should not be construed as a reference to the present invention. limits.
