Industrial robot trajectory planning method, electronic device and readable storage medium

Abstract

The invention discloses an industrial robot trajectory planning method, an electronic device and a readable storage medium, comprising the following steps: S1, constructing a robot terminal position and posture matrix; S2, calculating the joint rotation angles of the first three joints of the robot in the joint space; S3, using quaternions to represent the terminal postures of the robot at the starting point and the end point of the path, obtaining the terminal posture of the robot in a time series by quaternion interpolation, and obtaining the terminal posture rotation matrix of the robot and the posture angle corresponding to the quaternion interpolation point based on the above; S4, calculating the joint rotation angles of the last three joints based on the posture angle and the joint rotation angles of the first three joints; the posture changes of the robot planned by the invention are continuous and smooth, the terminal posture of the robot at any intermediate point of the path can meet the requirements, and the calculation amount is small.

Description

Technical Field

[0001] The present invention belongs to the field of robotics technology, and in particular relates to an industrial robot trajectory planning method, electronic equipment, and a readable storage medium. Background Art

[0002] Motion planning for industrial robots is a critical step in completing their tasks, significantly impacting their performance. Typically, motion planning for industrial robots is categorized into two types: joint-space planning and Cartesian planning, depending on the planning space. Common industrial robot handling and loading / unloading tasks are planned in the robot's joint space. Motion planning in joint space eliminates the need to consider singularities and ensures accurate poses at the start and end points of a path. However, it struggles to guarantee the poses of intermediate points along the path.

[0003] Robotic painting, welding, and other tasks have clear requirements for the position and posture of each point on the path. When planning a path, a feasible continuous path is usually planned in Cartesian space first. The continuous path is then discretized into a series of key points. The joint rotation angles corresponding to the key point positions are obtained by inverse kinematics. Finally, a suitable interpolation method is selected to perform joint interpolation in the joint space. The planning of such tasks needs to take singular points into account. Since the continuous path in Cartesian space needs to be discretized, the inverse kinematic solution must be calculated for all the key points obtained by discretization, which significantly increases the computational complexity of motion path planning. Summary of the Invention

[0004] The purpose of the embodiments of the present invention is to provide an industrial robot trajectory planning method that can not only ensure the accurate posture of the starting and ending points of the path, but also ensure the posture of each point in the middle of the path. It only requires the kinematic inverse solution calculation of the last three joints of all path points, which greatly reduces the amount of calculation.

[0005] Another object of the embodiments of the present invention is to provide an electronic device and a readable storage medium.

[0006] To solve the above technical problems, the technical solution adopted by the present invention is an industrial robot trajectory planning method, comprising the following steps:

[0007] S1, construct the robot's end pose matrix;

[0008] S2, obtain the position coordinates of the robot at the starting point and the ending point of the path, and calculate the joint angles of the robot's first three joints at the starting point and the ending point of the path;

[0009] Use the polynomial interpolation function to interpolate the joint angles of the first three joints of the robot to obtain the joint angle sequence of the first three joints in time;

[0010] S3, using quaternions to represent the end posture of the robot at the starting and ending points of the path, and obtaining the end posture Q of the robot in the time series based on the quaternion interpolation function t , construct attitude rotation matrix and attitude quaternion Q t Based on the conversion equation, the robot's end attitude rotation matrix and the attitude angle corresponding to the quaternion interpolation point in the time series are obtained;

[0011] S4, deform the end pose matrix of the robot based on the posture angle, substitute the joint angles of the first three joints, and obtain the joint angle sequence of the last three joints of the robot in time.

[0012] Furthermore, the joint rotation angles of the first three joints of the robot in S2 are calculated as follows:

[0013]

[0014]

[0015]

[0016] Where θ1, θ2, and θ3 represent the joint rotation angles of the first, second, and third joints, respectively, and p x 、p y 、p z They represent the position coordinates of the robot end in the reference coordinate system, k1, k2, k3, F1, and F2 are all intermediate variables. k3=p z -d1, k2 = [(k1) 2 +(k3) 2 +(a3) 2 -(a4) 2 -(d4) 2 ] / 2a3, F1=k1-a3c2, F2=k3-a3s2, a3 represents the length of the third connecting rod, a4 represents the length of the fourth connecting rod, d1 represents the offset of the first joint, d4 represents the offset of the fourth joint, c1 represents the cosine value of the joint angle of the first joint, c2 represents the cosine value of the joint angle of the second joint, s2 represents the sine value of the joint angle of the second joint.

[0017] Furthermore, the polynomial interpolation function in S2 is as follows:

[0018]

[0019] Where i represents the number of joints, i = 1, 2, 3, t represents the time point when the robot moves from the starting point to the end point, t = 0, 1, ..., f, f represents the total time the robot moves from the starting point to the end point, represents the joint rotation angle of the i-th joint at time point t, a i0 、a i1 、a i2 、a i3 、a i4 、a i5 They all represent the polynomial coefficients for interpolating the joint angle of the i-th joint.

[0020] Furthermore, the interpolation function in S3 is as follows:

[0021] Q t =Q 0 sin(Ω-Ωl(t)) / sinΩ+Q f sin(Ωl(t)) / sinΩ,t∈[0,f]

[0022] Q 0 represents the end posture of the robot at the starting point of the path, Q f Indicates the terminal posture of the robot at the end point of the path, Q t represents the end posture of the robot at time point t, l(t) represents the normalized time operator, and Ω represents Q 0 With Q f Angle;

[0023] Attitude rotation matrix and attitude quaternion Q t The conversion equation is as follows:

[0024]

[0025] Where q0, q1, q2, and q3 represent the quaternion Q t The four components of R t Indicates Q t The corresponding attitude rotation matrix.

[0026] Furthermore, the joint rotation angles of the last three joints in S4 are calculated as follows:

[0027]

[0028] θ4=arctan2(-r 33 ,r 13 ) or θ4=arctan2(r 33 ,-r 13 )

[0029] θ6=arctan2(-r 22 ,r 21 ) or θ6=arctan2(r 22 ,-r 21 )

[0030] Where θ4, θ5, and θ6 represent the joint rotation angles of the 4th, 5th, and 6th joints, respectively. 23 =sin(α t -θ1-θ2-θ3) / 2-sin(α t -θ1+θ2+θ3) / 2, r 33 =-s4s5, r 13 =c4s5,r 21 =c6s5,r 22 =-s5s6,α t represents the posture angle of the robot at time point t, s4 represents the sine value of the joint angle of the 4th joint, s5 represents the sine value of the joint angle of the 5th joint, c4 represents the cosine value of the joint angle of the 4th joint, c6 represents the cosine value of the joint angle of the 6th joint, and s6 represents the sine value of the joint angle of the 6th joint.

[0031] An electronic device comprises a processor, a memory and a communication bus, wherein the processor and the memory communicate with each other via the communication bus;

[0032] Memory for storing computer programs;

[0033] The processor is configured to implement the above method steps when executing the program stored in the memory.

[0034] A computer-readable storage medium stores a computer program, which implements the above method steps when executed by a processor.

[0035] The beneficial effects of the present invention are: the embodiment of the present invention can plan a feasible robot motion path between the starting point and the end point, it is easy to control the end posture of the robot, and is suitable for tasks with operational requirements for the end posture of the robot; the embodiment of the present invention plans the motion of the first three joints in the joint space, and obtains the motion planning of the last three joints through the robot end posture planning and kinematic inverse solution. There is no need to consider the robot singularity problem, and only the kinematic inverse solution calculation of the last three joints of the robot in all path points is required, which has a small amount of calculation and is easy to use. BRIEF DESCRIPTION OF THE DRAWINGS

[0036] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the following briefly introduces the drawings required for use in the embodiments or the description of the prior art. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying any creative work.

[0037] Figure 14 is a flow chart of trajectory planning according to an embodiment of the present invention.

[0038] Figure 2 It is a schematic diagram of the robot's structure and movements.

[0039] Figure 3 This is a schematic diagram of the robot's link coordinate system.

[0040] Figure 4 2 is a diagram of simulation results of an embodiment of the present invention. DETAILED DESCRIPTION

[0041] The following will clearly and completely describe the technical solutions in the embodiments of the present invention in conjunction with the accompanying drawings. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative efforts are within the scope of protection of the present invention.

[0042] Example

[0043] like Figure 1 As shown, the industrial robot trajectory planning method specifically includes the following steps:

[0044] S1, taking the ABB IRB1200 robot carrying a cup filled with liquid as an example, the robot trajectory planning method is verified. The DH parameters of the industrial robot are shown in Table 1:

[0045] Table 1 DH parameter table

[0046] Connecting rod <![CDATA[Joint rotation angle θ i > <![CDATA[Joint offset d i > <![CDATA[Connecting rod length a i > <![CDATA[Connecting rod torsional angle α i > 1 <![CDATA[θ1]]> <![CDATA[d1]]> 0 0 2 <![CDATA[θ2]]> 0 0 90° 3 <![CDATA[θ3]]> 0 <![CDATA[a3]]> 0 4 <![CDATA[θ4]]> <![CDATA[d4]]> <![CDATA[a4]]> -90° 5 <![CDATA[θ5]]> 0 0 90° 6 <![CDATA[θ6]]> 0 0 90°

[0047] Figure 2 The figure shows the structure of the IRB1200 robot. The reference coordinate system o0x0y0z0 is established at the bottom center of the robot base. The DH parameter pre-coordinate system method is used to establish the link coordinate system o on the IRB1200 robot. i x i y i z i (i=1,2,3,…,6), the connecting rod coordinate system is as follows Figure 3 As shown, the link coordinate system transformation matrix of the adjacent joints of the robot is obtained, as shown in formula (1):

[0048] T i i-1 =Rot(x i-1 ,α i-1 )Trans(a i-1 ,0,0)Rot(z i ,θ i)Trans(0,0,d i )(1)

[0049] T i i-1 Represents the connecting rod coordinate system o i-1 x i-1 y i-1 z i-1 To the connecting rod coordinate system o i x i y i z i The transformation matrix, a i-1 represents the length of the i-1th connecting rod, α i-1 represents the torsion angle of the i-1th connecting rod, θ i represents the joint rotation angle of the i-th joint, d i Indicates the offset of the i-th joint, Rot(x i-1 ,α i-1 ) represents the rotation around x i-1 Rotation angle α i-1 The rotation matrix, Trans(a i-1 ,0,0) means along x i-1 Translation a i-1 The movement matrix, Rot(z i ,θ i ) represents the rotation around z i Rotation angle θ i The rotation matrix, Trans(0,0,d i ) represents the direction along z i Translation distance d i The movement matrix.

[0050] Then the robot's end pose matrix Substitute the parameters in Table 1 into the robot end pose matrix We can get:

[0051]

[0052] in:

[0053] n x =s6×(c4×s1-s4×(c1×s2×s3-c1×c2×c3))-c6×(c5×(s1×s4+c4×(c1×s2×s3-c1×c2×c3))+s5×(c1×c2×s3+c1×c3×s2))

[0054] n y=c6×(c5×(c1×s4-c4×(s1×s2×s3-c2×c3×s1))-s5×(c2×s1×s3+c3×s1×s2))-s6×(c1×c4+s4×(s1×s2×s3-c2×c3×s1))

[0055] n z =c6×(s5×(c2×c3-s2×s3)+c4×c5×(c2×s3+c3×s2))+s4×s6×(c2×s3+c3×s2)

[0056] a y =-c6×(c1×c4+s4×(s1×s2×s3-c2×c3×s1))-s6×(c5×(c1×s4-c4×(s1×s2×s3-c2×c3×s1))-s5×(c2×s1×s3+c3×s1×s2))

[0057] a z =c6×s4×(c2×s3+c3×s2)-s6×(s5×(c2×c3-s2×s3)+c4×c5×(c2×s3+c3×s2))

[0058] A x =c5×(c1×c2×s3+c1×c3×s2)-s5×(s1×s4+c4×(c1×s2×s3-c1×c2×c3))

[0059] A y =s5×(c1×s4-c4×(s1×s2×s3-c2×c3×s1))+c5×(c2×s1×s3+c3×s1×s2)

[0060] A z =c4×s5×(c2×s3+c3×s2)-c5×(c2×c3-s2×s3)

[0061] p x =c1×(a4×c 23 -d4×s 23 +a3×c2)

[0062] p y =s1×(a4×c 23 -d4×s 23 +a3×c2)

[0063] p z =d1+d4×c 23 +a4×s 23 +a3×s2

[0064] ci =cosθ i , s i = sinθ i , c ij =cos(θ i +θ j ), s ij = sin(θ i +θ j ), (i, j = 1, 2, ..., 6), c i is the joint angle θ of the i-th node i The cosine value, c ij is the joint rotation angle θ of the i-th joint i The joint angle θ of the jth joint j The cosine of the sum, s i is the joint angle θ of the i-th node i The sine value, s ij is the joint rotation angle θ of the i-th joint i The joint angle θ of the jth joint j The sine of the sum.

[0065] S2, motion planning for the robot's first to third joints;

[0066] P x 、p z The calculation formula is transformed as shown below:

[0067]

[0068] d4×c 23 +a4×s 23 =p z -a3×s2-d1

[0069] The joint angles of the industrial robot are solved by matrix transformation and inverse transformation:

[0070]

[0071] By (p x ) 2 +(p z ) 2 We can get:

[0072] k1 cosθ2+k3 sinθ2=k2

[0073]

[0074] Among them, k1, k2, and k3 are intermediate variables. k3=p z -d1, k2 = [(k1)2 +(k3) 2 +(a3) 2 -(a4) 2 -(d4) 2 ] / 2a3, a3 represents the length of the third link, a4 represents the length of the fourth link, c1 represents the cosine value of the joint angle of the first joint, d1 represents the offset of the first joint, and d4 represents the offset of the fourth joint.

[0075] Substitute θ2 into p x and p z We can get:

[0076]

[0077] Among them, F1 and F2 are intermediate variables, F1 = k1-a3c2, F2 = k3-a3s2, c2 represents the cosine value of the joint angle of the second joint, and s2 represents the sine value of the joint angle of the second joint.

[0078] When the robot grabs a cup filled with liquid, assuming that the total time it takes for the robot to grab the cup from the starting point to the end point is f, the robot's terminal pose matrix is The robot's position coordinates at the starting point are known and the position coordinates of the end point Will Substitute into formula (2), (3), and (4) respectively to solve the joint rotation angles of the robot's first to third joints at the starting point and the joint rotation at the end point

[0079] Use the fifth-order polynomial interpolation function to interpolate the joint angles of the robot's first three joints to obtain the joint angle sequence of the first three joints corresponding to time. The fifth-order polynomial interpolation function is as follows:

[0080]

[0081] Where i represents the number of joints, i = 1, 2, 3, t represents the time point when the robot moves from the starting point to the end point, t = 0, 1, ..., f, represents the joint rotation angle of the i-th joint at time point t, a i0 、a i1 、a i2 、a i3 、a i4 、a i5 They all represent the polynomial coefficients for interpolating the joint angle of the i-th joint. The use of quintic polynomial interpolation can ensure the continuity of the robot joint motion displacement, angular velocity, and angular acceleration.

[0082] S3, motion planning for the robot's 4th to 6th joints;

[0083] In order to ensure that the liquid in the cup held by the end of the robot will not spill during the movement of the robot, the robot must always keep the y-axis of the end wrist joint coordinate system vertical when grasping, transporting and placing the cup, such as Figure 2 shown.

[0084] If the motion of joints 4 to 6 is still planned in the joint space, it can only ensure that the end posture of the starting and end points of the path meets the requirements, but the posture of the intermediate points of the path cannot be guaranteed. According to the Pieper criterion, the axes of the three adjacent joints of the industrial robot intersect at one point or are parallel to each other. Then, the inverse kinematic solution can obtain a certain number of closed solutions. For a six-degree-of-freedom robot whose last three joints are revolute joints and whose axes intersect at one point, the last three joints determine the posture of the end effector. Therefore, the motion planning of the last three nodes will directly affect the posture control effect of the robot transporting the cup. In order to achieve the goal of posture control of the robot end, consider posture planning of the robot end in Cartesian space, and then use the robot inverse solution to obtain the joint angles of joints 4 to 6 at each motion moment.

[0085] In order to avoid the problems of jump points, universal joint lock and singularity caused by the rotation matrix and Euler angle method for posture planning, the unit quaternion method is used to plan the posture of the robot end, using quaternion Q 0 , Q f represents the terminal posture of the robot at the starting and ending points of the path, Q 0 =[o0,o1,o2,o3], Q f =[f0,f1,f2,f3],o m Represents the quaternion Q 0 The four components of f n Represents the quaternion Q f The four components of m, n = 1, 2, 3, 4;

[0086] Q 0 With Q f The angle between them is Ω, then cosΩ=Q 0 Q f , the interpolation function of the attitude quaternion is:

[0087] Q t =Q 0 sin(Ω-Ωl(t)) / sinΩ+Q f sin(Ωl(t)) / sinΩ,t∈[0,f] (5)

[0088] Q trepresents the end posture of the robot at time point t, l(t) represents the normalized time operator;

[0089] At any time, the attitude rotation matrix R t and attitude quaternion Q t =The conversion equation between [q0,q1,q2,q3] is:

[0090]

[0091] q0, q1, q2, q3 represent quaternion Q respectively t The four components of the robot terminal posture rotation matrix sequence in the time series can be obtained through formulas (5) and (6), and then the posture angle α corresponding to the quaternion interpolation point can be obtained. t ;

[0092] Because the robot is in the process of grabbing, transporting and placing the cup, the y-axis of the robot's end wrist joint coordinate system always remains vertically upward, and a rotation angle α is allowed around the y-axis. t , so relative to the reference coordinate system of the robot base, the robot end posture matrix is ​​expressed as:

[0093]

[0094] p x ′、p y ′、p z ′ respectively represent the specific position coordinate values ​​of the robot end in the reference coordinate system.

[0095] Depend on Available The left side of the equation is:

[0096]

[0097] The right side of the equation is:

[0098]

[0099] According to formula (7), we have:

[0100] r 23 =sin(α t -θ1-θ2-θ3) / 2-sin(α t -θ1+θ2+θ3) / 2

[0101] r 13 =cos(α t -θ1-θ2-θ3) / 2+cos(α t -θ1+θ2+θ3) / 2

[0102] r33 =-sin(α t -θ1)

[0103] r 22 =cos(θ2+θ3)

[0104] r 21 =cos(α t -θ1-θ2-θ3) / 2-cos(α t -θ1+θ2+θ3) / 2

[0105] By r 23 =-c5, we can get

[0106] By r 13 =c4s5,r 33 =-s4s5 to obtain θ4=arctan2(-r 33 ,r 13 ) or θ4=arctan2(r 33 ,-r 13 );

[0107] By r 21 =c6s5,r 22 =-s5s6 to obtain θ6 = arctan2(-r 22 ,r 21 ) or θ6=arctan2(r 22 ,-r 21 );

[0108] According to the planning of the first three joints, θ1, θ2, θ3, and α corresponding to each moment are known. The above formula can be used to obtain the joint angles θ4, θ5, and θ6 of joints 4 to 6 at each moment.

[0109] In summary, since there are multiple sets of solutions for joint angles, the method used in selection is: first find all solutions, then determine the valid solutions based on the motion range of each joint, and then select the optimal solution closest to the joint angle of the robot at the previous moment from the valid solutions. The path planned by this method not only meets the posture requirements of the starting and end points of the path, but also the posture of the robot end at the middle point of the path meets the requirements, and the posture changes of the robot end are continuous and smooth.

[0110] The quaternion method is used here to plan the posture of the end effector, obtain the posture angle of the end effector with the same number of quaternion interpolation points, and then combine the angles of the first three joints to obtain the joint angles of the last three joints. From this process, it can be seen that the angles of the last three joints are related to the posture of the end effector and the angles of the first three joints. The path planning obtained based on this is smoother.

[0111] The robot's grasping point coordinates are (675.34, 19.45, 736), and the placement point coordinates are (150, 700, 596.5). The robot's end rotation angle at the grasping point is 0, and the rotation angle at the placement point is α = 77.1373°. As mentioned above, after the robot grasps the cup until it puts it down at the placement point, the y-axis of its end wrist joint coordinate system always remains vertically upward. The aforementioned motion planning method is used in Matlab to simulate the robot's motion from the starting point to the placement point. The motion time t is 1s, and the interpolation interval is 0.02s. The simulation results are as follows Figure 4 As shown by Figure 4 It can be seen that the y-axis of the robot's end wrist joint coordinate system always remains vertically upward during the movement.

[0112] Of course, if you want to control the robot's end joints to be in other postures, for example, if you want to control the z-axis of the robot's end coordinate system o6x6y6z6 to be vertical, then Should be:

[0113]

[0114] That is, if you want to control the robot's end joints to be in other postures, just modify the robot's end posture matrix according to the control requirements. A similar method is then used to calculate the joint angles of the robot's rear three joints, thereby obtaining the robot's path planning trajectory.

[0115] The present invention also includes an electronic device, including a memory and a processor, wherein the memory is used to store various computer program instructions, and the processor is used to execute the computer program instructions to complete all or part of the above steps; the electronic device can communicate with one or more external devices, and can also communicate with one or more devices that enable users to interact with the electronic device, and / or communicate with any device that enables the electronic device to communicate with one or more other computing devices. The electronic device can also communicate with one or more networks (such as local area networks, wide area networks and / or public networks) through a network adapter.

[0116] The present invention also includes a computer-readable storage medium storing a computer program that can be executed by a processor. The computer-readable storage medium may include but is not limited to magnetic storage devices, optical disks, digital versatile disks, smart cards and flash memory devices. In addition, the readable storage medium described in the present invention can represent one or more devices and / or other machine-readable media for storing information. The term "machine-readable medium" includes but is not limited to wireless channels and various other media (and / or storage media) that can store, contain and / or carry codes and / or instructions and / or data.

[0117] Each embodiment in this specification is described in a related manner. Similar parts between the various embodiments can be referred to in conjunction with each other. Each embodiment focuses on the differences between the other embodiments. In particular, the system embodiment is generally similar to the method embodiment, so the description is relatively simple. For related parts, refer to the description of the method embodiment.

[0118] The above description is only a preferred embodiment of the present invention and is not intended to limit the scope of protection of the present invention. Any modifications, equivalent replacements, improvements, etc. made within the spirit and principles of the present invention are included in the scope of protection of the present invention.

Claims

1. An industrial robot trajectory planning method, characterized in that: The following steps are involved: S1, construct the robot's end pose matrix; S2, obtaining the position coordinates of the robot at the starting point and the ending point of the path, and calculating the joint angles of the first three joints of the robot at the starting point and the ending point of the path; Use the polynomial interpolation function to interpolate the joint rotation angles of the first three joints of the robot to obtain the joint rotation angle sequence of the first three joints in time; S3, using quaternions to represent the end posture of the robot at the starting and ending points of the path, and obtaining the end posture Q of the robot in the time series based on the quaternion interpolation function t , construct the attitude rotation matrix and attitude quaternion Q t Based on the conversion equation, the robot's end attitude rotation matrix and the attitude angle corresponding to the quaternion interpolation point in the time series are obtained; S4, deforming the terminal pose matrix of the robot based on the pose angle, substituting the joint rotation angles of the first three joints, and obtaining the joint rotation angle sequence of the last three joints of the robot in time; The joint rotation angles of the first three joints of the robot in S2 are calculated as follows: Where θ1, θ2, and θ3 represent the joint rotation angles of the first, second, and third joints, respectively, and p x 、p y 、p z They represent the position coordinates of the robot end in the reference coordinate system. k1, k2, k3, F1, and F2 are all intermediate variables. k3=p z -d1, Indicates the length of the third connecting rod, represents the length of the fourth connecting rod, d1 represents the offset of the first joint, d4 represents the offset of the fourth joint, c1 represents the cosine value of the joint angle of the first joint, c2 represents the cosine value of the joint angle of the second joint, and s2 represents the sine value of the joint angle of the second joint; The polynomial interpolation function in S2 is as follows: Where i is a variable representing the number of joints, i=1, 2, 3, t represents the time point when the robot moves from the starting point to the end point, t=0, 1, ..., f, f represents the total time it takes for the robot to move from the starting point to the end point, represents the joint rotation angle of the i-th joint at time point t, All represent the polynomial coefficients for interpolating the joint angle of the i-th joint; The joint rotation angles of the last three joints in S4 are calculated as follows: θ4=arctan2(-r 33 ,r 13 ) or θ4=arctan2(r 33 ,-r 13 ) θ6=arctan2(-r 22 ,r 21 ) or θ6=arctan2(r 22 ,-r 21 ) Where θ4, θ5, and θ6 represent the joint rotation angles of the 4th, 5th, and 6th joints, respectively. 23 = sin(α t -θ1-θ2-θ3) / 2-sin(α t -θ1+θ2+θ3) / 2, r 33 =-s4s5,r 13 =c4s5,r 21 =c6s5,r 22 =-s5s6, α t represents the posture angle of the robot at time point t, s4 represents the sine value of the joint angle of the 4th joint, s5 represents the sine value of the joint angle of the 5th joint, c4 represents the cosine value of the joint angle of the 4th joint, c6 represents the cosine value of the joint angle of the 6th joint, and s6 represents the sine value of the joint angle of the 6th joint.

2. The industrial robot trajectory planning method according to claim 1, characterized in that: The interpolation function in S3 is as follows: Q t =Q 0 sin(Ω-Ωl(t)) / sinΩ+Q f sin(Ωl(t)) / sinΩ,t∈[0,f] Q 0 represents the end posture of the robot at the starting point of the path, Q f represents the terminal posture of the robot at the end point of the path, Q t represents the end posture of the robot at time point t, l(t) represents the normalized time operator, and Ω represents Q 0 With Q f The angle of Attitude rotation matrix and attitude quaternion Q t The conversion equation is as follows: Where q0, q1, q2, and q3 represent the quaternion Q respectively. t The four components of R t Indicates Q t The corresponding attitude rotation matrix.

3. An electronic device, characterized in that: It includes a processor, a memory and a communication bus, wherein the processor and the memory communicate with each other via the communication bus; Memory, used to store computer programs; A processor, for implementing the method steps described in any one of claims 1-2 when executing a program stored in a memory.

4. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores a computer program, and when the computer program is executed by a processor, the method steps described in any one of claims 1-2 are implemented.

Images