Mechanical arm motion trail optimization method based on timing interpolation and transportation mechanical arm

Abstract

The invention provides a mechanical arm motion trail optimization method based on timing interpolation and a transportation manipulator, and the method comprises the steps: S1, dividing the motion of a mechanical arm into a plurality of synchronous periodic motions through a timing interpolation mode, and calculating the displacement and speed of the mechanical arm in each synchronous periodic motion; s2, a kinematic model of the mechanical arm is established through an improved DH parameter method; s3, the synchronous periodic motion is split according to inverse kinematics, and an inverse solution is solved for a key path point of the motion of the mechanical arm, so that a first change joint angle corresponding to the key path point is obtained; and S4, judging the first change joint angle according to a preset dynamic programming algorithm, and carrying out segmentation optimization on the synchronous periodic motion to obtain the joint angular velocity of the optimal solution and the segmentation times of the optimal solution. The synchronous performance of the transmission manipulator can be effectively improved, the contour tracking precision is effectively improved, and then the effect of effectively reducing shaking in the moving process of the manipulator is achieved.

Description

Technical Field

[0001] The present invention relates to the technical field of industrial robot motion control, and in particular to a method for optimizing a robot arm motion trajectory based on timing interpolation and a transport robot arm. Background Art

[0002] Integrated circuits (ICs) are the core of the electronic information industry and one of the high-tech technologies that promote the development of national economic and social informatization. IC manufacturing equipment technology is the basic technology for the rapid development of the IC industry. The wafer transfer system is an indispensable component of IC equipment, and the wafer transfer robot is a key component of the wafer transfer system, which is mainly responsible for the precise positioning of the wafer and the fast and stable transportation of the wafer.

[0003] During the wafer processing, even the slightest friction on the edge of the wafer will cause damage to the wafer. The wafer transfer robot is mainly responsible for transferring wafers between various workstations. In order to smoothly pick up and place wafers and avoid friction between the wafer box and the wafer, the deviation of the trajectory taken by the end of the robot relative to the expected trajectory should be as small as possible, that is, the trajectory profile of the end effector should be accurate, and the movement should be as smooth and vibration-free as possible.

[0004] In view of the above problems existing in the prior art, it is urgent to design a robot arm motion trajectory optimization method based on timed interpolation to solve the above problems. Summary of the invention

[0005] In order to overcome the technical problems existing in the prior art, the present application provides a method for optimizing the motion trajectory of a robotic arm based on timed interpolation, thereby being able to solve the problems raised in the above-mentioned background technology.

[0006] The present application provides a method for optimizing the motion trajectory of a robotic arm based on timed interpolation, comprising:

[0007] Step S1: dividing the motion of the robot arm into a number of synchronous periodic motions by means of timed interpolation, and calculating the displacement and speed of the robot arm in each synchronous periodic motion;

[0008] Step S2: establishing a kinematic model of the robotic arm by using an improved DH parameter method;

[0009] Step S3: splitting the synchronous periodic motion according to inverse kinematics through the kinematic model, and finding an inverse solution for the key path points of the robot motion in the rectangular coordinate space to obtain a first changed joint angle corresponding to the key path points;

[0010] Step S4: judging the first changed joint angle according to a preset dynamic programming algorithm, and segmenting and optimizing the synchronous periodic motion to obtain the joint angular velocity of the optimal solution and the number of segmentations of the optimal solution.

[0011] In some embodiments, step S4 comprises:

[0012] Step S41: when the first changed joint angle is greater than a preset value, further dividing the synchronous periodic motion corresponding to the changed joint angle into a plurality of stable synchronous motions according to the optimal solution division times;

[0013] Step S42: splitting the stable synchronous motion according to inverse kinematics, and obtaining an inverse solution for the key path points of the robot motion in the rectangular coordinate space to obtain a second changed joint angle corresponding to the key path points;

[0014] Step S43: judging the second changed joint angle according to a preset dynamic programming algorithm, and obtaining the joint angular velocity of the optimal solution when the second changed joint angle is less than a preset value.

[0015] The synchronous periodic motion with excessive joint angle changes is further divided into several stable synchronous motions by timed interpolation, so that the joint angular velocity in the stable synchronous motion is lower than the preset value, thereby effectively avoiding excessive jitter of the robot arm.

[0016] In some embodiments, the mechanical arm includes a large arm, a small arm and an end effector, and the large arm, the small arm and the end effector are respectively provided with a large arm joint motor, a small arm joint motor and an end effector joint motor;

[0017] The robot arm motion trajectory optimization method realizes control of the motion trajectory of the robot arm by communicating with the upper arm joint motor, the lower arm joint motor and the end effector joint motor respectively through an Ethernet communication line.

[0018] In some implementations, step S2 further includes:

[0019] Step S21: splitting the robot arm motion into upper arm motion, lower arm motion and end effector motion according to the inverse kinematics;

[0020] The step S3 also includes: obtaining an initial joint angle and a final joint angle corresponding to the key path point, wherein the changed joint angle is calculated based on the initial joint angle and the final joint angle.

[0021] In some embodiments, the initial joint angle and the final joint angle are both the joint angles of the joint at the current moment;

[0022] The joint angle is acquired based on the angle encoder;

[0023] Alternatively, the joint angle is predicted based on the historical angle of the corresponding joint in the robotic arm.

[0024] In some embodiments, based on the kinematic model, the position coordinates of the end effector are: Wherein θ1, θ2, θ3 are the current joint angles of the upper arm, the lower arm and the end effector respectively, and L1, L2, L3 are the lengths of the upper arm, the lower arm and the end effector respectively.

[0025] In some implementations, the communication synchronization period of the synchronous periodic motion and the stable synchronous motion is 1 ms.

[0026] In some embodiments, based on the preset dynamic programming algorithm, the speed curve of the robotic arm is an S-shaped speed curve.

[0027] In some embodiments, the present application further provides a transport robot arm, comprising:

[0028] A joint with six degrees of freedom and a control device, wherein a preset computer program is stored in the control device. When the computer program is executed by the control device, the transport robot arm executes the method described in any of the above embodiments.

[0029] The present invention has at least the following beneficial effects:

[0030] In the technical solution provided in the present application, the robot arm with excessive movement amplitude is further reasonably segmented through timed interpolation and a suitable control algorithm to obtain the joint angular velocity of the optimal solution, so as to reduce the speed of the synchronous periodic motion corresponding to the changing joint angle, effectively improve the synchronization performance of the transmission robot, effectively improve the contour tracking accuracy, and thereby achieve the effect of effectively reducing the jitter during the movement of the robot arm, while making the transportation movement of the robot arm smoother. BRIEF DESCRIPTION OF THE DRAWINGS

[0031] Figure 1 A schematic diagram of the flow of the robot arm motion trajectory optimization method based on timed interpolation of the present application;

[0032] Figure 2 This is a schematic diagram of the subdivision process of S4 of the robot arm motion trajectory optimization method based on timed interpolation of the present application;

[0033] Figure 3 A schematic diagram of coordinates of the motion of a robotic arm according to the method for optimizing the motion trajectory of a robotic arm based on timed interpolation of the present application;

[0034] Figure 4 This is a speed curve change diagram of the robotic arm of the robotic arm motion trajectory optimization method based on timed interpolation in this application. DETAILED DESCRIPTION

[0035] The following specific embodiments illustrate the implementation of the present application, and those skilled in the art can easily understand other advantages and effects of the present application from the contents disclosed in this specification. Although the description of the present application will be introduced in conjunction with some embodiments, this does not mean that the features of this application are limited to the implementation. On the contrary, the purpose of introducing the application in conjunction with the implementation is to cover other options or modifications that may be extended based on the claims of the present application. In order to provide a deep understanding of the present application, the following description will include many specific details. The present application can also be implemented without using these details. In addition, in order to avoid confusion or blurring the focus of the present application, some specific details will be omitted in the description. It should be noted that the embodiments in the present application and the features in the embodiments can be combined with each other without conflict.

[0036] It should be noted that in this specification, similar reference numerals and letters denote similar items in the following drawings, and therefore, once an item is defined in one drawing, it does not need to be further defined and explained in the subsequent drawings.

[0037] In the description of the present application, it should be noted that the terms "center", "upper", "lower", "left", "right", "vertical", "horizontal", "inner", "outer", etc., indicating the orientation or positional relationship, are based on the orientation or positional relationship shown in the drawings, and are only for the convenience of describing the present application and simplifying the description, rather than indicating or implying that the device or element referred to must have a specific orientation, be constructed and operated in a specific orientation, and therefore cannot be understood as limiting the present application. In addition, the terms "first" and "second" are used for descriptive purposes only and cannot be understood as indicating or implying relative importance.

[0038] In the description of this application, it should be noted that, unless otherwise clearly specified and limited, the terms "installed", "connected", and "connected" should be understood in a broad sense, for example, it can be a fixed connection, a detachable connection, or an integral connection; it can be a mechanical connection or an electrical connection; it can be a direct connection, or it can be indirectly connected through an intermediate medium, or it can be the internal communication of two components. For ordinary technicians in this field, the specific meanings of the above terms in this application can be understood according to specific circumstances.

[0039] This application provides a method for optimizing the motion trajectory of a robotic arm based on timed interpolation. Figure 1, including step S1: dividing the motion of the robot arm into a number of synchronous periodic motions by means of timed interpolation, and calculating the displacement and speed that the robot arm needs to perform in each synchronous periodic motion;

[0040] Step S2: Establishing the kinematic model of the robot arm by using the improved DH parameter method;

[0041] Step S3: splitting the synchronous periodic motion according to inverse kinematics through the kinematic model, and finding an inverse solution for the key path points of the robot motion in the rectangular coordinate space to obtain the first changed joint angle corresponding to the key path points;

[0042] Step S4: judging the first changed joint angle according to a preset dynamic programming algorithm, and performing segmentation optimization on the synchronous periodic motion to obtain the joint angular velocity of the optimal solution and the number of segmentation times of the optimal solution.

[0043] Specifically, the present application is applicable to SCARA type wafer transfer robots, but is not limited to this type of robot. Timing interpolation is to divide the movement time into several equal unit times, and calculate the path and speed that the robot arm needs to move in each unit time. Finally, the inverse kinematics solution is used to calculate the displacement and speed of the robot arm in each unit time. Then the controller sends motor commands at the preset time to enable the robot arm to move according to the preset route and speed, so that the robot arm can move more smoothly and stably.

[0044] In some embodiments, see Figure 2 , step S4 comprises:

[0045] Step S41: when the first changed joint angle is greater than a preset value, the synchronous periodic motion corresponding to the changed joint angle is further divided into a plurality of stable synchronous motions according to the optimal solution division times;

[0046] Step S42: splitting the stable synchronous motion according to inverse kinematics, and obtaining an inverse solution for the key path points of the robot motion in the rectangular coordinate system space to obtain a second changed joint angle corresponding to the key path points;

[0047] Step S43: judging the second changed joint angle according to a preset dynamic programming algorithm, and obtaining the joint angular velocity of the optimal solution when the second changed joint angle is less than a preset value.

[0048] Specifically, when the robotic arm performs synchronous periodic motion after being segmented by timed interpolation, continuous synchronous periodic motion will cause a sudden change in the speed of the robotic arm, which may easily lead to vibration of the robotic arm. Therefore, the synchronous periodic motion with excessive joint angle changes is further segmented into several stable synchronous motions according to the method of timed interpolation, so that the joint angular velocity in the stable synchronous motion is lower than the preset value, thereby effectively avoiding excessive jitter of the robotic arm. The number of subdivisions of the synchronous periodic motion is set according to the change in the joint angle per unit time.

[0049] In some embodiments, the robot arm includes a large arm, a small arm and an end effector, and the large arm, the small arm and the end effector are respectively provided with a large arm joint motor, a small arm joint motor and an end effector joint motor;

[0050] The robot arm motion trajectory optimization method realizes control of the robot arm's motion trajectory by communicating with the upper arm joint motor, the lower arm joint motor and the end effector joint motor through Ethernet communication lines.

[0051] Specifically, a lifting platform and three joints are provided in the robotic arm. The upper arm, the lower arm and the end effector are connected through the joints, and the upper arm is set on the lifting platform through the joints to achieve relative rotation. The upper arm joint motor, the lower arm joint motor and the end effector joint motor can all control the corresponding parts on the robotic arm based on their respective joint motor currents. This method can be executed by a robotic arm control device. The robotic arm control device can be implemented in the form of hardware and / or software. The robotic arm control device can be configured in electronic devices such as robotic arms, computers and servers. The upper arm joint motor, the lower arm joint motor and the end effector joint motor are all servo motors. The joint motor can automatically correct the motor current acting on the joint motor based on real-time feedback of the robotic arm information to achieve the maintenance of the balance state of the robotic arm. The robotic arm information includes one or more of the joint angle, robotic arm position, robotic arm speed and robotic arm strength.

[0052] It should be pointed out that the joints in the robotic arm can be rotating ball joints or rotating slider joints, but are not limited to these two types of joints. The specific situation depends on the actual situation and is not limited here.

[0053] In some embodiments, step S2 further includes:

[0054] Step S21: splitting the robot arm motion into upper arm motion, lower arm motion and end effector motion according to inverse kinematics;

[0055] Step S3 also includes: obtaining an initial joint angle and a final joint angle corresponding to the key path point, and the changed joint angle is calculated based on the initial joint angle and the final joint angle.

[0056] Specifically, see Appendix Figure 2 The working principle of the transport robot that performs tasks according to the robot arm motion trajectory optimization method based on timed interpolation provided in this application is specifically as follows: L1 corresponds to driving the upper arm to rotate, L2 corresponds to driving the lower arm to rotate, and the linear motion is achieved by the upper arm and the lower arm cooperating at a certain angle, and L3 corresponds to driving the terminal end effector to rotate. The three axes of the upper arm, the lower arm, and the terminal end effector can achieve a specific trajectory and posture of the end point through coordinated and synchronous motion. The transport robot moves at a preset operating speed. Within a unit of time, the end effector on the robot moves from point P to point P′. The joint angle of the upper arm L1 corresponding to the position of point P is θ1, the joint angle of the lower arm L2 is θ2, and the joint angle of the end effector L3 is θ3; the joint angle of the upper arm L1 corresponding to the position of point P′ is θ 1′ , the joint angle of forearm L2 is θ 2′ , the joint angle of the end effector L3 is θ 3′ Therefore, the joint angle change of the upper arm L1 is φ1 = θ 1′ -θ1, the joint angle change of forearm L2 φ2 = θ 2′ -θ3, the joint angle change of the end effector L3 φ3 = θ 3′ -θ3.

[0057] When the transport robot moves from point P to point P' at certain specific angles in unit time, the angle changes φ1, φ2, or φ3 of the upper arm, lower arm, and end effector are too large, which will cause excessive vibration of the silicon carbide wafer on the end effector. In order to reduce the vibration amplitude, it is necessary to reduce the angle changes φ1, φ2, and φ3 of the upper arm, lower arm, and end effector in unit time to a certain range. Specifically, a number of unit times are inserted between points P and P' according to the angle change. Each unit time corresponds to a new point, so that the movement of the robot is divided into movements with smaller angle changes. See Figure 3 As shown, a point P″ is inserted between points P and P′. At the point P″, the angle changes φ4, φ5, and φ6 corresponding to the upper arm, lower arm, and end effector are calculated by inverse kinematics. It is found that the three angle changes will be reduced relative to the angle corresponding to P′, thereby reducing the jitter amplitude of the robot arm and effectively improving the stability of the robot arm's movement.

[0058] In some embodiments, the initial joint angle and the final joint angle are both the joint angles of the joint at the current moment;

[0059] The joint angle is acquired based on the angle encoder;

[0060] Alternatively, the joint angles are predicted based on the historical angles of the corresponding joints in the robotic arm.

[0061] Specifically, each joint position on the robotic arm is provided with an angle encoder. The angle encoder is a device for measuring the rotation angle, including a rotating component and a photoelectric sensor. By setting the angle encoder, high-precision joint angles can be collected in real time, thereby ensuring the timeliness and accuracy of the joint angles of each joint in the robotic arm, which helps to achieve precise control of the robotic arm and makes the joint angle results obtained quick and easy. In addition, based on the existing experience and knowledge accumulation and the pre-set angle prediction model, the corresponding joint angle change trend can be predicted based on the historical angle of at least one joint, and then the corresponding joint angle can be obtained, thereby providing a basis and guarantee for subsequent optimization segmentation and jitter reduction.

[0062] In some embodiments, based on the kinematic model, the position coordinates of the end effector are: Among them, θ1, θ2, θ3 are the current joint angles of the upper arm, lower arm and end effector respectively, and L1, L2, L3 are the lengths of the upper arm, lower arm and end effector respectively.

[0063] Specifically, L1, L2, L3, the end point (x, y) and the posture β are all known conditions. Then, through the position coordinates of the end effector, θ1, θ2 and θ3 can be calculated by reverse calculation, and it can be known that:

[0064]

[0065] This provides a guarantee for the next step, thereby ensuring that the robot arm motion trajectory optimization method provided in this application can be implemented.

[0066] In some embodiments, the communication synchronization period of the synchronous periodic motion and the stable synchronous motion is 1 ms.

[0067] Specifically, by setting the communication synchronization period of the synchronous periodic motion and the stable synchronous motion to 1ms, it is possible to achieve high-precision control of the position of the end effector of the robot arm, so that the position accuracy of reaching the target position can reach the millimeter level or even smaller. In addition, it can better capture and respond to changes in the motion process, effectively avoid position deviations caused by inertia and other factors, and effectively improve the real-time and responsiveness of the motion process, thereby making the motion trajectory of the robot arm smoother, thereby achieving the effect of reducing jitter and improving stability.

[0068] In some embodiments, see Figure 4 As shown, based on the preset dynamic programming algorithm, the speed curve of the robotic arm is an S-shaped speed curve.

[0069] Specifically, most traditional robotic arms have a trapezoidal speed curve, which changes linearly during the acceleration and deceleration stages. There is a large impact force due to the sudden change in acceleration, which in turn causes large jitter during the movement of the robotic arm. The acceleration of the S-shaped speed curve in the robotic arm changes continuously, and its speed transition and change are more natural than the traditional trapezoidal speed curve, and can have a more stable motion process and processing accuracy.

[0070] In some embodiments, the present application further provides a transport robot arm, comprising a joint with six degrees of freedom, and a control device, wherein a preset computer program is stored in the control device. When the computer program is executed by the control device, the transport robot arm executes a method as in any of the above embodiments.

[0071] Specifically, a transport robot arm that executes the method described in any of the above embodiments further reasonably divides the robot arm with excessive movement amplitude through timed interpolation and adopts a suitable control algorithm to obtain the joint angular velocity of the optimal solution, so as to reduce the speed of the synchronous periodic motion corresponding to the changing joint angle, effectively improve the synchronization performance of the wafer transfer robot, effectively improve the contour tracking accuracy, and thus achieve the effect of effectively reducing the jitter during the movement of the robot arm, while making the transport movement of the robot arm smoother.

[0072] The wafer transfer robot provided in this application is mainly used for wafer picking and wafer placement. Here, taking wafer picking is taken as an example to illustrate the working process of the robot. The wafer placement process is the opposite of the wafer picking process. The wafer picking process of the robot is as follows:

[0073] First, the upper arm, lower arm, end gripping device and lifting device operate simultaneously to make the robot arm reach a specific height and angle, ensuring that the end effector can be inserted into the wafer box and maintain an appropriate distance from the wafer; then the upper arm, lower arm and end gripping device continue to move in a coordinated manner to insert into the wafer box, and then prepare for wafer removal; then the lifting device rises a specific distance to allow the end effector to contact the wafer in the wafer box; then the solenoid valve is opened to start the vacuum adsorption function so that the wafer is adsorbed on the end effector; finally, the upper arm, lower arm and end gripping device operate in a coordinated manner to allow the wafer to follow the end effector out of the wafer box.

[0074] Obviously, those skilled in the art can make various changes and modifications to the present application without departing from the spirit and scope of the present application. Thus, if these modifications and modifications of the present application fall within the scope of the claims of the present application and their equivalents, the present application is also intended to include these modifications and modifications.

Claims

1. A method for optimizing the motion trajectory of a robotic arm based on timed interpolation, characterized in that: include: Step S1: dividing the motion of the robot arm into a number of synchronous periodic motions by means of timed interpolation, and calculating the displacement and speed of the robot arm in each synchronous periodic motion; Step S2: establishing a kinematic model of the robotic arm by using an improved DH parameter method; Step S3: splitting the synchronous periodic motion according to inverse kinematics through the kinematic model, and finding an inverse solution for the key path points of the robot motion in the rectangular coordinate space to obtain a first changed joint angle corresponding to the key path points; Step S4: judging the first changed joint angle according to a preset dynamic programming algorithm, and segmenting and optimizing the synchronous periodic motion to obtain the joint angular velocity of the optimal solution and the number of segmentations of the optimal solution.

2. The method for optimizing the motion trajectory of a robotic arm based on timed interpolation according to claim 1, characterized in that: The step S4 comprises: Step S41: when the first changed joint angle is greater than a preset value, further dividing the synchronous periodic motion corresponding to the changed joint angle into a plurality of stable synchronous motions according to the optimal solution division times; Step S42: splitting the stable synchronous motion according to inverse kinematics, and obtaining an inverse solution for the key path points of the robot motion in the rectangular coordinate space to obtain a second changed joint angle corresponding to the key path points; Step S43: judging the second changed joint angle according to a preset dynamic programming algorithm, and obtaining the joint angular velocity of the optimal solution when the second changed joint angle is less than a preset value.

3. The method for optimizing the motion trajectory of a robotic arm based on timed interpolation according to claim 2, characterized in that: The mechanical arm comprises a large arm, a small arm and an end effector, wherein the large arm, the small arm and the end effector are respectively provided with a large arm joint motor, a small arm joint motor and an end effector joint motor; The robot arm motion trajectory optimization method realizes control of the motion trajectory of the robot arm by communicating with the upper arm joint motor, the lower arm joint motor and the end effector joint motor respectively through an Ethernet communication line.

4. The method for optimizing the motion trajectory of a robotic arm based on timed interpolation according to claim 3, characterized in that: The step S2 further comprises: Step S21: splitting the robot arm motion into upper arm motion, lower arm motion and end effector motion according to the inverse kinematics; The step S3 also includes: obtaining an initial joint angle and a final joint angle corresponding to the key path point, wherein the changed joint angle is calculated based on the initial joint angle and the final joint angle.

5. The method for optimizing the motion trajectory of a robot arm based on timed interpolation according to claim 4, characterized in that: The initial joint angle and the final joint angle are both the joint angles of the joint at the current moment; The joint angle is acquired based on the angle encoder; Alternatively, the joint angle is predicted based on the historical angle of the corresponding joint in the robotic arm.

6. The method for optimizing the motion trajectory of a robot arm based on timed interpolation according to claim 5, characterized in that: Based on the kinematic model, the position coordinates of the end effector are: Wherein θ1, θ2, θ3 are the current joint angles of the upper arm, the lower arm and the end effector respectively, and L1, L2, L3 are the lengths of the upper arm, the lower arm and the end effector respectively.

7. The method for optimizing the motion trajectory of a robot arm based on timed interpolation according to claim 6, characterized in that: The communication synchronization period of the synchronous periodic motion and the stable synchronous motion is 1 ms.

8. The method for optimizing the motion trajectory of a robot arm based on timed interpolation according to any one of claims 1 to 7, characterized in that: Based on the preset dynamic programming algorithm, the speed curve of the robotic arm is an S-shaped speed curve.

9. A transport robot arm, characterized in that: include: A joint with six degrees of freedom, and a control device, wherein a preset computer program is stored in the control device. When the computer program is executed by the control device, the transport robot arm executes the method as described in any one of claims 1-8.

Images