CN104827481B - SCARA manipulator control method based on motion controller - Google Patents

Abstract

The invention provides a SCARA manipulator control method based on a motion controller, which comprises the motion controller and an industrial touch screen, wherein a user plans a motion track of the SCARA manipulator and corresponding motion parameters through the touch screen, and stores the motion track and the parameters into the motion controller; and the motion controller calculates the actual motion track of the SCARA manipulator according to the algorithm corresponding to the track planning mode, and simulates the motion track in a rectangular coordinate system established by the virtual axis. According to the invention, the motion trail of the SCARA manipulator and the corresponding motion parameters are directly compiled and planned through the touch screen, the virtual rectangular coordinate system is constructed by adopting the virtual axis, and the interpolation algorithm in the motion controller is utilized, so that the calculated amount can be greatly reduced, the motion algorithm is effectively simplified, the development difficulty of the SCARA control system technology is reduced, the development period is shortened, and the cost is lower.

Description

A Control Method of SCARA Manipulator Based on Motion Controller

technical field

The invention relates to the technical field of manipulator control, in particular to a motion controller-based SCARA manipulator control method.

Background technique

With the development of industrialization and the improvement of production automation level, industrial robots are used more and more widely; they play an increasingly important role in industrial applications such as welding, handling, assembly, spraying, and palletizing. At present, SCARA manipulators are widely used in electronics, automobiles, plastics, food and other industrial fields, and its main function is to complete handling and assembly work. With the increasing complexity and precision of processing technology, SCARA manipulators often need to cooperate with other industrial equipment on the assembly line, and it is inevitable that there will be a danger of collision with obstacles. Therefore, it is particularly important to plan the trajectory of the SCARA manipulator with obstacles in the working range. Trajectory planning refers to the obstacle conditions of the given environment, as well as the location of the starting point and the target point. It is required to choose a path from the starting point to the target point so that the SCARA manipulator can pass through all obstacles safely and without collision.

Existing SCARA manipulators generally adopt an architecture system based on embedded control system + motion control chip, such as embedded DSP, which has a long development cycle, pulse control mode, and weak anti-interference ability; each movement to a different movement position, The position of each interpolation point must be calculated according to the interpolation accuracy, which is cumbersome and heavy workload; and it needs to be given a specified position by the host computer (such as a computer, etc.) to be able to act. It can be seen that the SCARA control system in the prior art has high technical difficulty, long development cycle and high cost.

Contents of the invention

The present invention provides a motion controller-based SCARA manipulator control method for the problems in the prior art. By combining the motion controller and industrial touch screen, the motion algorithm is effectively simplified, thereby realizing the control of the SCARA manipulator and solving the problem of the existing SCARA control system. High technical difficulty, long development cycle, and high cost.

In order to solve the above technical problems, the present invention adopts the following technical solutions:

A kind of SCARA manipulator control method based on motion controller, comprises motion controller and industrial touch screen, and described motion controller communicates with touch screen through Ethernet communication port, and control method step is as follows:

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 to the motion controller;

Step 2: The motion controller calculates the actual trajectory of the SCARA manipulator according to the algorithm corresponding to the trajectory planning method, simulates its trajectory in the Cartesian coordinate system established by the virtual axis, and obtains the Cartesian coordinates , and then the Cartesian coordinates of the virtual axis Converted to the rotation coordinates of the SCARA manipulator ;

Step 3: Get the rotation coordinates of the SCARA manipulator After the position, the current position of each motor axis with the manipulator Compare to get the difference , and calculate the position and rotation speed of each motor shaft in the next cycle according to the difference;

Step 4: The motion controller adopts the electronic cam control mode, assigns the position and speed obtained in step 3 to the electronic cam parameters, rotates to the target position at a given speed, obtains the actual motion track information of the SCARA manipulator, and transfers the actual The motion trajectory information is sent to the SCARA manipulator.

Preferably, the trajectory in step 1 includes point-to-point, linear interpolation and circular interpolation trajectory.

Further, when the motion track is a point-to-point motion, the parameters include the end point coordinates, the rotation speed and the posture of the SCARA manipulator; when the motion track is a linear interpolation motion, the parameters include the end point coordinates and Interpolation speed; when the motion track is circular interpolation motion, the parameters include end point coordinates, circle center coordinates or arc radius or points on the arc and arc interpolation speed.

Still further, the step of converting the virtual axis Cartesian coordinates into rotating coordinates in the step 2 is as follows: Step 2.1: When the motion track is linear interpolation, then in the virtual coordinate system, according to the specified acceleration and 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;

Step 2.2: When the motion track is circular interpolation, start the virtual axis in the form of circular interpolation in the virtual coordinate system according to the specified acceleration and deceleration, speed and center or radius or points on the circular arc Move from the current position to the end position, and then perform step 2.3;

Step 2.3: In the Cartesian coordinate system established by the virtual axis, move the virtual axis to the end point coordinates according to the trajectory and parameters given in step 2.1 or step 2.2; at the same time, open each scan cycle to obtain the position of the virtual axis, and the obtained position of the virtual axis Converted to the rotational position of each motor shaft of the SCARA manipulator .

Wherein, the specific conversion method of the step 2.3 is as follows:

Step 2.3.1: Put converted to ,

Let the arm lengths of the first arm and the second arm of the SCARA manipulator be A and B respectively, then the side length of the hypotenuse C is ,

According to the law of cosines

,

;

Step 2.3.2: Determine the quadrant where the target point is located

when , the target point is in the first and second quadrants, and the angle between the hypotenuse and the coordinate system is ;

when , the target point is in the fourth quadrant, and the angle between the hypotenuse and the coordinate system is ;

when , the target point is in the third quadrant, and the angle between the hypotenuse and the coordinate system is ;

Step 2.3.3: When the SCARA manipulator is in the right hand posture, , ;

When the SCARA manipulator is in the left-hand posture, , ;

Step 2.3.4: For the u-axis, when taking Cartesian coordinates as the reference system, ;

When the z-axis is a screw structure, there is no need to change, .

Beneficial effects of the present invention:

A control method for a SCARA manipulator based on a motion controller of the present invention directly writes and plans the motion trajectory of the SCARA manipulator and its corresponding motion parameters through the touch screen, uses virtual axes to construct a virtual Cartesian coordinate system, and utilizes interpolation inside the motion controller Algorithm, can greatly reduce the amount of calculation, effectively simplify the motion algorithm, reduce the difficulty of developing SCARA control system technology, shorten the development cycle, and the cost is low.

Description of drawings

Fig. 1 is a schematic structural view of the SCARA manipulator described in the present invention.

FIG. 2 is a flow chart of the specific realization of the control method of the four-degree-of-freedom SCARA manipulator based on the motion controller of the present invention.

Fig. 3 is a solution diagram of the rotation angle of the SCARA manipulator described in the present invention.

FIG. 4 is a schematic diagram of a case where the touch screen performs a planned motion trajectory.

Fig. 5 is a schematic diagram of linear interpolation performed by the four-degree-of-freedom SCARA manipulator of the present invention.

Fig. 6 is a diagram of the speed and position changes of the four-degree-of-freedom SCARA manipulator in the present invention when performing linear interpolation.

detailed description

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 accompanying drawings, and the contents mentioned in the embodiments are not intended to limit the present invention. Referring to Fig. 1 to Fig. 6, the present invention will be described in detail below in conjunction with the accompanying drawings.

A SCARA manipulator control method based on a motion controller provided by the present invention includes a motion controller and an industrial touch screen. The touch screen has secondary development functions such as position display, teaching, route planning, and general IO input and output. The motion controller communicates with the touch screen through the Ethernet communication port, and the steps of the control method are as follows:

Step 1: The user plans the trajectory of the SCARA manipulator and its corresponding motion parameters through the touch screen, and saves the trajectory and parameters to the motion controller; preferably, the trajectory in step 1 includes point-to-point, Linear interpolation and circular interpolation motion trajectory. The SCARA manipulator of the present invention includes two rotating arms, and an execution shaft is arranged on the second rotating arm, which has four motor shafts in total.

As shown in Figure 4, it is a kind of motion track case of the present invention; In the first line, the speed of specifying point-to-point is 50% of the maximum speed; The second line, the set point-to-point acceleration and deceleration time is 300ms; The third line specifies the speed of linear interpolation and circular interpolation as 2000mm/s; the fourth line sets the acceleration and deceleration time of linear interpolation and circular interpolation as 500ms; the fifth line plans the SCARA manipulator to point-to-point The method moves to the position of (0,300,0,0), and the SCARA manipulator is in the right-hand posture when moving to the target point; the sixth line, the SCARA manipulator moves from the current position (0,300,0,0) to the target position in a straight line ( 300,300,0,0), and the SCARA manipulator maintains the posture of the starting position during the movement, that is, the right-hand posture; in the seventh line, the SCARA manipulator draws a circle clockwise from the current position (300,300,0,0) in the form of circular interpolation From the arc to (300,300,0,0), the arc radius R is 50mm, that is, draw a clockwise circle with a radius of 50mm, and the manipulator maintains the posture of the starting point during the movement, that is, the right-hand posture.

Step 2: The motion controller calculates the actual trajectory of the SCARA manipulator according to the algorithm corresponding to the trajectory planning method, simulates its trajectory in the Cartesian coordinate system established by the virtual axis, and obtains the Cartesian coordinates , and then the Cartesian coordinates of the virtual axis Converted to the rotation coordinates of the SCARA manipulator .

Further, when the motion track 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 track is a linear interpolation motion, the parameters include the end point Coordinates and interpolation speed; when the motion track is arc interpolation motion, the parameters include end point coordinates, center coordinates or arc radius or points on the arc and arc interpolation speed.

Still further, the step of converting the virtual axis Cartesian coordinates into rotating coordinates in the step 2 is as follows: Step 2.1: When the motion track is linear interpolation, then in the virtual coordinate system, according to the specified acceleration and 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;

Step 2.2: When the motion track is circular interpolation, start the virtual axis in the form of circular interpolation in the virtual coordinate system according to the specified acceleration and deceleration, speed and center or radius or points on the circular arc Move from the current position to the end position, and then perform step 2.3;

Step 2.3: In the Cartesian coordinate system established by the virtual axis, move the virtual axis to the end point coordinates according to the trajectory and parameters given in step 2.1 or step 2.2; at the same time, open each scan cycle to obtain the position of the virtual axis, and the obtained position of the virtual axis Converted to the rotational position of each axis of the SCARA manipulator .

Wherein, the specific conversion method of the step 2.3 is as follows:

Step 2.3.1: Put converted to ,

Let the arm lengths of the first arm and the second arm of the SCARA manipulator be A and B respectively, then the side length of the hypotenuse C is ,

According to the law of cosines

,

;

Step 2.3.2: Determine the quadrant where the target point is located

when , the target point is in the first and second quadrants, and the angle between the hypotenuse and the coordinate system is ;

when , the target point is in the fourth quadrant, and the angle between the hypotenuse and the coordinate system is ;

when , the target point is in the third quadrant, and the angle between the hypotenuse and the coordinate system is ;

Step 2.3.3: When the SCARA manipulator is in the right hand posture, , ;

When the SCARA manipulator is in the left-hand posture, , ;

Step 2.3.4: For the u-axis, when taking Cartesian coordinates as the reference system, ;

When the z-axis is a screw structure, there is no need to change, .

In this embodiment, take the linear interpolation motion trajectory as an example; the linear interpolation motion trajectory of the fifth row in Fig. 4 is an example here; in this motion trajectory only axis movement, The axes are not moving, as shown in Figure 5, the SCARA manipulator moves from P1 to P2. When starting the manipulator, start the virtual axis first , so that it moves from the current position to the target position in a straight line according to the specified speed and acceleration and deceleration time in the established Cartesian coordinate system. In this example, the The axis will move in a straight line from P1 to P2 according to the speed and position shown in Figure 6.

At the same time, the motion controller acquires The position of the axis, in the lower figure of Figure 5, where some two moments and read to axis position and ; combine get and ; Substitute it into Get side lengths:

,

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

,

,

because , the target point is in the first and second quadrants, and the angle between the hypotenuse and the coordinate system is ,Right now

,

Since SCARA is in the right hand posture, , which is

,

, .

Step 3: Get the rotation coordinates of the SCARA manipulator After the position, the current position of each motor axis with the manipulator Compare to get 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 with the position of the previous cycle 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; where is the scan cycle.

Step 4: The motion controller adopts the electronic cam control mode, assigns the position and speed obtained in step 3 to the electronic cam parameters, rotates to the target position at a given speed, obtains the actual motion track information of the SCARA manipulator, and transfers the The actual motion trajectory information is sent to the SCARA manipulator. Directly put the position obtained in step 3 , and speed , As a parameter of the electronic cam control method, the motor shaft will move from the current position to the specified speed. , The position of the manipulator can move from the point Pi in the virtual axis to the point Pi+1, and the distance from Pi to Pi+1 is small enough, so that the trajectory of the manipulator from Pi→Pi+1 can be approximated as a straight line. The P1→P2 straight line is composed of many short straight lines of Pi→Pi+1, so as to realize the straight line motion of the manipulator from P1→P2.

To sum up the above, a kind of SCARA manipulator control method based on the motion controller of the present invention, it directly writes and plans the SCARA manipulator motion trajectory and its corresponding motion parameters through the touch screen, and uses the touch screen to write the user program and specify the working position; Using the virtual axis to construct a virtual 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 the cost is low .

The above content is only a preferred embodiment of the present invention. For those of ordinary skill in the art, according to the idea of the present invention, there will be changes in the specific implementation and application scope. limits.

Claims (4)

1. a kind of SCARA manipulator control method based on motion controller, comprises motion controller and industrial touch screen, and described motion controller communicates with touch screen by Ethernet communication port, it is characterized in that: control method step is as follows: 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 to the motion controller; Step 2: The motion controller calculates the actual trajectory of the SCARA manipulator according to the algorithm corresponding to the trajectory planning method, simulates its trajectory in the Cartesian coordinate system established by the virtual axis, and obtains the Cartesian coordinates , and then the Cartesian coordinates of the virtual axis Converted to the rotation coordinates of the SCARA manipulator ; Among them, the steps of converting virtual axis Cartesian coordinates into rotating coordinates 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; Step 2.2: When the motion track is circular interpolation, start the virtual axis in the form of circular interpolation in the virtual coordinate system according to the specified acceleration and deceleration, speed and center or radius or points on the circular arc Move from the current position to the end position, and then perform step 2.3; Step 2.3: In the Cartesian coordinate system established by the virtual axis, move the virtual axis to the end point coordinates according to the trajectory and parameters given in step 2.1 or step 2.2; at the same time, open each scan cycle to obtain the position of the virtual axis, and the obtained position of the virtual axis Converted to the rotational position of each motor shaft of the SCARA manipulator ; Step 3: Get the rotation coordinates of the SCARA manipulator After the position, the current position of each motor axis with the manipulator Compare to get the difference , and calculate the position and rotation speed of each motor shaft in the next cycle according to the difference; Step 4: The motion controller adopts the electronic cam control method, assigns the position and speed of each motor shaft in the next cycle obtained in step 3 to the electronic cam parameters, rotates to the target position at a given speed, and obtains the actual motion track information of the SCARA manipulator , and send the actual trajectory information to the SCARA manipulator. 2. A kind of SCARA manipulator control method based on motion controller according to claim 1, is characterized in that: the motion trajectory in the described step 1 comprises point-to-point, linear interpolation and circular interpolation motion trajectory. 3. A kind of SCARA manipulator control method based on motion controller according to claim 1, is characterized in that: when described motion locus is point-to-point motion, described parameter comprises terminal point coordinate, rotational speed and SCARA manipulator attitude; when the motion track is a linear interpolation motion, the parameters include end point coordinates and interpolation speed; when the motion track is circular interpolation motion, the parameters include end point coordinates, center coordinates or circle Arc radius or point on the arc and circular interpolation speed. 4. a kind of SCARA manipulator control method based on motion controller according to claim 1, is characterized in that: described step 2.3 specific conversion method is as follows: Step 2.3.1: Put converted to , Let the arm lengths of the first arm and the second arm of the SCARA manipulator be A and B respectively, then the side length of the hypotenuse C is , According to the law of cosines , ; Step 2.3.2: Determine the quadrant where the target point is located when , the target point is in the first and second quadrants, and the angle between the hypotenuse and the coordinate system is ; when , the target point is in the fourth quadrant, and the angle between the hypotenuse and the coordinate system is ; when , the target point is in the third quadrant, and the angle between the hypotenuse and the coordinate system is ; Step 2.3.3: When the SCARA manipulator is in the right hand posture, , ; When the SCARA manipulator is in the left-hand posture, , ; Step 2.3.4: For the u-axis, when taking Cartesian coordinates as the reference system, ; When the z-axis is a screw structure, there is no need to change, .