Multi-connecting-rod manipulator trajectory planning method based on spatial three-dimensional coordinates

Abstract

The invention discloses a multi-connecting-rod manipulator trajectory planning method based on spatial three-dimensional coordinates, and relates to the field of automatic control. The method comprises a multi-connecting-rod model conversion module, a linear trajectory planning module and an equipment interlocking module. Bidirectional conversion of a joint angle and a three-dimensional coordinate is realized through multi-connecting-rod model conversion; based on coordinates of a starting point and an end point, adopting seven S-shaped curves to plan global cooperation time, generating a smooth trajectory in combination with PVT interpolation, and dynamically converting jerk to ensure accurate displacement; bidirectional interlocking of the mechanical arm and the station equipment is achieved by presetting a safety area and judging the position in real time. The method does not need to depend on a transverse external shaft, the reachable range of the mechanical arm is expanded, vibration can be effectively restrained, collision accidents are avoided, the method is suitable for precise load transfer in the multi-station clean environment such as EFEM, and the problems that a traditional polar coordinate control method is poor in flexibility and low in safety are solved.

Description

Technical Field

[0001] This invention relates to the field of automation control technology, and in particular to a method for planning the trajectory of a multi-link manipulator based on three-dimensional spatial coordinates. Background Technology

[0002] In fields such as semiconductor manufacturing, where environmental cleanliness and operational precision are extremely important, cleanroom robotic arms are the core equipment for automating the transfer of precision loads such as wafers.

[0003] Currently, there are three main configurations of cleanroom robotic arms used on a large scale. The first is the SCARA (Selective Compliance Assembly Robot Arm) configuration, which is a single arm and single finger or multiple arms and multiple fingers. It is usually used with linear motors as external axes. The second is the multi-axis coupling configuration with two arms and two fingers. The third is the configuration with independent slides for multiple axes, which is generally used for large equipment.

[0004] Currently, all existing trajectory planning and control methods for multi-link cleanroom manipulators are based on a polar coordinate system. That is, the motion coordinate system is constructed with the zero position of the manipulator as the origin, the rotation direction as the θ axis, and the extension direction as the R axis. This control method has significant limitations, such as limited reach and attitude adjustment capabilities. It cannot achieve full coverage by relying solely on the body structure. When dealing with the operation of multi-station, approximately cuboid EFEM (Equipment Front End Module) equipment, linear motors must be used as external axes to supplement the degree of freedom of lateral movement. This results in additional motors and drivers occupying internal space and reducing the integration of the equipment.

[0005] Meanwhile, the introduction of external axes requires the software system to be adapted to different versions without external axes, which increases the complexity of development and maintenance. Vibration is prone to occur during multi-axis linkage, and the difficulty of vibration control increases significantly with the number of axes, affecting motion accuracy and equipment stability. Summary of the Invention

[0006] To address the aforementioned issues, the present invention aims to overcome the shortcomings of existing multi-link manipulator trajectory planning and control methods based on polar coordinates. It provides a multi-link atmospheric manipulator trajectory planning method based on three-dimensional spatial coordinates. This method is compatible with multi-link cleanroom manipulators and eliminates the need for an external axis for lateral movement. It achieves safe and efficient entry and exit paths to workstations solely through the coordinated operation of multiple arm segments within the manipulator body, enabling precise load transfer within a multi-station EFEM. Furthermore, through multi-link model conversion, linear trajectory planning, and equipment interlocking mechanisms, the stability, safety, and reliability of the motion process are ensured.

[0007] To achieve the above objectives, the present invention adopts the following technical solution:

[0008] A method for planning the trajectory of a multi-link manipulator based on three-dimensional spatial coordinates includes the following steps:

[0009] Step 1: Establish a bidirectional conversion model between the polar coordinates of each axis joint of the multi-link manipulator and the three-dimensional spatial coordinates of the end effector;

[0010] Step 2: Based on the three-dimensional coordinates of the starting and ending points of the multi-link manipulator's end effector, a linear motion trajectory is generated by combining 7-segment S-shaped velocity-time curve planning with PVT interpolation, and the position-velocity-time PVT data set of each planned point in the linear motion trajectory is output.

[0011] Step 3: The PVT data group is synchronously output to the joint drivers of each axis of the multi-link manipulator via the industrial bus, so as to control the joints of the multi-link manipulator to move in coordination according to the planned trajectory.

[0012] Step 4: Based on the comparison results between the real-time 3D coordinates of the end effector and the preset safety zone, construct a two-way interlock mechanism between the robot and the external equipment;

[0013] Furthermore, the construction process of the weekend conversion model described in step 1 includes:

[0014] Position signals are collected by absolute encoders installed at the drive ends of each axis, and combined with axis reduction ratio and transmission efficiency parameters to convert them into joint values ​​for each axis as polar coordinates of each axis joint. The joint values ​​include: joint angles of the rotating axis joints of each arm of the multi-link robot and vertical height values ​​of the lifting axis.

[0015] A bidirectional transformation model between the polar coordinates of each axis joint and the three-dimensional coordinates of the end effector is constructed using forward kinematics.

[0016] Furthermore, the generation of the linear motion trajectory in step 2 includes the following:

[0017] Step 2.1: Calculate the actual displacement S;

[0018] The motion start coordinates (X1, Y1, Z1) and end coordinates (X2, Y2, Z2) of the end effector are obtained by a bidirectional transformation model. The displacements ΔX=|X2-X1|, ΔY=|Y2-Y1|, and ΔZ=|Z2-Z1| in each dimension of the three-dimensional coordinate system are calculated to form the actual displacement S=(ΔX, ΔY, ΔZ).

[0019] Step 2.2: Setting motion constraint parameters;

[0020] Independent motion constraint parameters are set for each of the X, Y, and Z directions in the three-dimensional spatial coordinate system. The motion constraint parameters include: maximum velocity V, maximum jerk A, and jerk J.

[0021] Step 2.3: Motion time-displacement planning;

[0022] The following are calculated for each of the X, Y, and Z directions in three-dimensional spatial coordinates: the duration t1 and displacement S1 of the acceleration segment, the duration t2 and displacement S2 of the uniform acceleration segment, the duration t3 and displacement S3 of the uniform speed segment, the duration t4 and displacement S4 of the deceleration segment, the duration t5 and displacement S5 of the uniform deceleration segment, and the duration t6 and displacement S6 of the deceleration-deceleration segment. The sum of the maximum values ​​of the corresponding motion segment times in each direction is taken as the global collaborative planning time t7.

[0023] Step 2.4: Dynamically calculate the actual jerk J based on the jerk J. （实际） ;

[0024] Displacement S planned based on motion segments in each direction （计算） According to the conversion factor K=S / S （计算） Update actual jerk J （实际） =J×K, where S （计算） =S1+S2+S3+S4+S5+S6;

[0025] Step 2.5: PVT interpolation and velocity calibration;

[0026] The linear motion trajectory is divided into N planning points according to the set control period T. By performing inverse kinematics on the bidirectional conversion model, the polar coordinates of each axis joint corresponding to the three-dimensional coordinates of the end effector at each planning point are obtained, and a position sequence P(n) is established, n=1,2,…,N. The position-time and velocity-time relationship model is constructed using cubic polynomial PVT interpolation. Finally, the precise velocity V(n) and position P(n) of each planning point T(n) are output, forming a complete PVT data set.

[0027] Furthermore, in step 2.3, the duration and displacement of each motion segment satisfy the following:

[0028] The duration of the acceleration segment is t1 = A / J, and the displacement of the acceleration segment is S1 = A. 3 / (6J 2 );

[0029] Duration t2 of uniform acceleration segment: If A 2 If / (2J)≥V, then t2=0; otherwise The displacement of the uniformly accelerated segment is S2 = A × t1 × t2 + (1 / 2) × A × t2 2 ;

[0030] Duration of the uniform velocity segment t3: If 2×(S1+S2)≥S, then t3=0; otherwise, t3=(S-2×(S1+S2)) / V. Displacement of the uniform acceleration segment S3=t3×V.

[0031] The duration of the deceleration / acceleration phase is t4=t1, and the displacement of the deceleration / acceleration phase is S4=S1.

[0032] The duration of the uniform deceleration segment is t5 = t2, and the displacement of the uniform deceleration segment is S5 = S2.

[0033] The duration of the deceleration / acceleration phase is t6=t1, and the displacement of the deceleration / acceleration phase is S6=S. 1；

[0034] Furthermore, the location-time relationship model in step 2.5 is constructed as follows:

[0035] First, for any two adjacent planning points P(n) and P(n+1), a position-time relationship model P(t) is constructed using a cubic polynomial. The position-time relationship model P(t) satisfies the following equation:

[0036] P(t=at) 3 +bt 2 +ct+d(t∈[T(n),T(n+1)]);

[0037] In the formula, a, b, c, and d are all coefficients;

[0038] Then, the position-time relationship model is set to satisfy the following boundary conditions: P(T(n))=P(n), P(T(n+1))=P(n+1), V(T(n))=V(n), V(T(n+1))=V(n+1), and the initial velocity V(1)=0, and the final velocity V(N)=0;

[0039] Substituting the above boundary conditions into the position-time relationship model, and taking the first derivative of the position-time relationship model with respect to time, V(t) = 3at. 2 +2bt+c forms a system of four linear equations. The coefficients a, b, c, and d are solved by matrix operations, and finally the position-time relationship model P(t) is obtained.

[0040] The speed-time relationship model is constructed as follows:

[0041] First, the coarse velocity of the intermediate planning point is calculated using the central difference method. For the nth planning point, where 2≤n≤N-1, the formula for calculating its coarse velocity V0(n) satisfies:

[0042] V0(n)[P(n+1)-P(n-1)] / (2T);

[0043] Set the coarse velocity V0(1) = 0 at the starting planning point and the coarse velocity V0(n) = 0 at the ending planning point;

[0044] Then, based on the position sequence P(n) and the coarse velocity V0(n), a velocity-time relationship model V(t) is constructed using a cubic polynomial.

[0045] Furthermore, the bidirectional interlocking mechanism described in step 4 is specifically as follows:

[0046] First, define the safe zone of the multi-link manipulator in a three-dimensional coordinate system.

[0047] Then, the three-dimensional coordinates of the end effector of the multi-link robot are collected in real time. If the current coordinates of the end effector are within the safe area, the external device can move freely.

[0048] If the current coordinates are exceeded, the coordinates of each workstation will be compared and traversed. External devices at workstations close to the current location will be locked, while external devices at other workstations will be able to move freely.

[0049] Compared with the prior art, the beneficial effects of the present invention are as follows:

[0050] (1) Using a multi-link configuration, there is no need for an external shaft that moves laterally, making the movement more flexible, the reach greater, and making it easier to suppress vibration and other problems by adjusting electrical parameters;

[0051] (2) Using straight-line trajectory planning for entering and exiting workstations makes the movement path simpler and reduces the risk of scratches;

[0052] (3) The atmospheric hand is interlocked with the external equipment, and the movement trajectory of the atmospheric hand is monitored in real time to avoid accidents such as collisions caused by equipment misoperation. Attached Figure Description

[0053] Figure 1 A schematic diagram of the overall structure of the multi-link manipulator adapted to this invention;

[0054] Figure 2 This is a flowchart of the present invention;

[0055] In the diagram: 1. Lifting shaft; 2. Upper arm rotation shaft; 3. Middle arm rotation shaft; 4. Forearm rotation shaft; 5. Lower finger rotation shaft; 6. Upper finger rotation shaft. Detailed Implementation

[0056] The technical solutions adopted in this invention will be clearly and completely explained and described below with reference to the accompanying drawings and specific embodiments;

[0057] First, the clean air multi-link manipulator adapted to this invention has a 6-axis configuration, such as... Figure 1 As shown, from bottom to top, it includes: lifting shaft 1 ( Figure 1 It consists of two lifting columns, upper arm rotating shaft 2, middle arm rotating shaft 3, forearm rotating shaft 4, lower finger rotating shaft 5, and upper finger rotating shaft 6;

[0058] This configuration of the robotic arm replaces the "rotation + extension" motion mode of the traditional polar coordinate robotic arm by coordinating the rotation of the multi-segment arm rotation axis and the vertical movement of the lifting axis 1, thus expanding the entire range of motion and posture adjustment capability of the robotic arm without external axes.

[0059] Secondly, the trajectory planning method for the aforementioned multi-link manipulator provided by this invention, such as... Figure 2 As shown, the specific steps include:

[0060] Step 1: Establish a bidirectional conversion model between the polar coordinates of each axis joint of the multi-link manipulator and the three-dimensional spatial coordinates of the end effector;

[0061] This involves using a bidirectional transformation model to achieve bidirectional conversion between the polar coordinates of each axis joint of a multi-link manipulator and the three-dimensional coordinates of the end effector. Specifically, this includes the following steps:

[0062] Step 1.1: Obtain the joint values ​​of each axis joint of the multi-link robot;

[0063] In this embodiment, position signals are collected by absolute encoders installed at the drive ends of each axis joint, and converted into joint values ​​of each axis joint by combining the axis reduction ratio and transmission efficiency parameters. The joint values ​​of each axis joint of the multi-link robot include: the joint angle of each arm rotation axis joint of the robot and the vertical height value of the lifting axis 1.

[0064] Step 1.2: Construct a bidirectional transformation model between the polar coordinates of each joint and the three-dimensional coordinates of the end effector using forward kinematics;

[0065] Specifically, the upper arm, middle arm, forearm and fingers of the multi-link manipulator are simplified into a four-link geometric model. The center of the manipulator base is taken as the origin of the coordinate system. The absolute angle is determined by accumulating the relative angles of each link, i.e. the joint values. The coordinates of each arm endpoint are calculated sequentially according to the method of "coordinate of the previous link endpoint ± component of the current link". Finally, the spatial three-dimensional coordinates of the end effector (taking the center of the finger above as an example) are obtained.

[0066] In the specific calculation process, first define the simplified symbols for angle cosine and sine:

[0067] , ;

[0068] Where T1 is the absolute angle of the boom rotation axis 2;

[0069] , ;

[0070] Where T2 is the rotation angle of the middle arm rotation axis 3 relative to the upper arm, and T1+T2 is the absolute angle of the middle arm rotation axis 3;

[0071] , ;

[0072] Where T3 is the rotation angle of the forearm rotation axis 4 relative to the middle arm, and T1+T2+T3 is the absolute angle of the forearm rotation axis 4;

[0073] , ;

[0074] Where T4 is the rotation angle of the upper finger rotation axis 6 relative to the forearm, and T1+T2+T3+T4 is the absolute angle of the finger rotation axis 6;

[0075] The center of the finger above is the reference point for the end effector, and its three-dimensional coordinates (X, Y, Z) are calculated using the following formula:

[0076] (1);

[0077] In formula (1), L1 is the length of the upper arm, L2 is the length of the middle arm, L3 is the length of the forearm, and L4 is the length of the upper finger. In this embodiment, the upper arm length L1 = 0.4m, the middle arm length L2 = 0.3m, the forearm length L3 = 0.25m, and the upper finger length L4 = 0.15m.

[0078] T5 is the joint value of lifting shaft 1, that is, the Z-axis coordinate is directly equivalent to the vertical height value of lifting shaft 1. Since lifting shaft 1 is independently controlled by a single motor, its joint value T5 has a linear correspondence with the Z-coordinate.

[0079] Using the target's three-dimensional coordinates as input, a system of nonlinear equations is constructed based on the forward equation. The numerical iteration method is then used to solve for the target joint angles of each axis and the target height of the lifting axis 1, ensuring that the joint values ​​are within the mechanical limit range.

[0080] Step 2: Based on the three-dimensional coordinates of the starting and ending points of the end effector's motion, a smooth linear motion trajectory is generated using a combination of 7-segment S-shaped velocity-time curve planning and PVT (Position Velocity Time) interpolation. This includes the following steps:

[0081] Step 2.1: Calculate the actual displacement S;

[0082] The bidirectional conversion model is obtained through step 1. The coordinates of the starting point (X1, Y1, Z1) and the ending point (X2, Y2, Z2) of the end effector are obtained. The actual displacement of the end effector in each dimension in the three-dimensional direction is calculated as S = (ΔX, ΔY, ΔZ), where: ΔX = |X2-X1|, ΔY = |Y2-Y1|, ΔZ = |Z2-Z1|.

[0083] In this embodiment, the joint values ​​of the multi-link manipulator corresponding to the starting point of the end effector's motion are: T1=0°, T2=30°, T3=-30°, T4=0°, T5=1.0m. Using formula (1), the starting point coordinates (X1, Y1, Z1) are obtained as (0.3m, 0.2m, 1.0m).

[0084] The coordinates of the end effector's motion endpoint are set as (X2, Y2, Z2) = (1.1m, 0.9m, 1.3m). The actual displacements of the end effector in each dimension in the three-dimensional direction are: ΔX = 0.8m, ΔY = 0.7m, ΔZ = 0.3m.

[0085] Step 2.2: Setting motion constraint parameters;

[0086] Set independent motion constraint parameters for the X, Y, and Z directions, including: maximum speed V = (V x V Y V Z ), Maximum acceleration (deceleration) A = (A x A Y A Z ) and jerk J = (J x J Y J Z ), where: V x A x J x Represented as motion constraint parameters in the X-axis direction; V Y A Y J Y Represented as motion constraint parameters in the Y-axis direction; V Z A Z J Z Represented as motion constraint parameters in the Z-axis direction;

[0087] In this embodiment, the dynamic constraint parameters in the X, Y, and Z directions are as follows:

[0088] X direction: V x =0.8m / s, A x =0.5m / s², J x =0.6m / s³;

[0089] Y direction: VY =0.7m / s, A Y =0.4m / s², J Y =0.5m / s³;

[0090] Z direction: V Z =0.5m / s, A Z =0.3m / s², J Z =0.4m / s³;

[0091] Step 2.3: Motion time-displacement planning;

[0092] In this embodiment, the seven S-shaped velocity-time curves are divided sequentially from the starting point to the ending point of the end effector's motion as follows:

[0093] The program consists of an acceleration segment, a uniform acceleration segment, a constant speed segment, a deceleration segment, a uniform deceleration segment, a deceleration-deceleration segment, and a global collaborative planning segment, where: the duration of the acceleration segment is t1, the duration of the uniform acceleration segment is t2, the duration of the constant speed segment is t3, the duration of the deceleration segment is t4, the duration of the uniform deceleration segment is t5, the duration of the deceleration-deceleration segment is t6, and the duration of the global collaborative planning segment is t7.

[0094] Furthermore, based on the motion constraint parameters and actual displacement S in each direction, the durations of the acceleration segment (t1), uniform acceleration segment (t2), uniform velocity segment (t3), deceleration segment (t4), uniform deceleration segment (t5), and deceleration-deceleration segment (t6) in each direction are calculated. The sum of the maximum durations of the corresponding motion segments in each direction is taken as the duration t7 of the global collaborative planning segment to ensure consistent motion rhythm across multiple directions.

[0095] The duration t1 of the acceleration phase satisfies: ;

[0096] This stage is represented by an acceleration that increases linearly from 0 to A, and the displacement during this stage is S1 = (1 / 6) × J × t1. 3 =A 3 / (6J 2 );

[0097] Duration t of the uniform acceleration segment 2， First, determine if the current speed has reached the maximum speed V: If A 2 If / (2J)≥V, then there is no uniform acceleration segment, i.e., t2=0; otherwise, The displacement at this stage is S2 = A × t1 × t2 + (1 / 2) × A × t2 2 ;

[0098] Duration t3 of the uniform velocity segment (S-curve plateau segment): Due to the total displacement S during the acceleration process (acceleration segment + uniform acceleration segment) a =S1+S2, the deceleration process is symmetrical to the acceleration process, that is, the total displacement S during the deceleration process is... d =S a If S a If ×2≥S, then there is no uniform velocity segment (t3=0); otherwise, the displacement of the uniform velocity segment is S3=S-2×S a t3 = S3 / V;

[0099] The deceleration and acceleration periods t4 and t5 are completely symmetrical. Therefore, t4 = t1 and t5 = t2; the corresponding displacements are S4 = S1 and S5 = S2.

[0100] The deceleration-acceleration phase t6 is a buffer phase in which the acceleration linearly decreases from 0 to A and then rises back to 0. The jerk changes linearly between J and -J. t6=t1, and the displacement S6=S1, ensuring that the velocity and acceleration are both 0 at the end of the motion.

[0101] Therefore, after calculating t1~t6 for each direction, the maximum value of the corresponding motion segment time in the X, Y, and Z directions is taken as the global collaborative planning time t7, i.e., t7 = max(Xt1, Yt1, Zt1) + max(Xt2, Yt2, Zt2) + max(Xt3, Yt3, Zt3) + max(Xt4, Yt4, Zt4) + max(Xt5, Yt5, Zt5) + max(Xt6, Yt6, Zt6); where (Xt1, Yt1, Zt6) = max(Xt2, Yt2, Zt2) + max(Xt3, Yt3, Zt3) + max(Xt4, Yt4, Zt4) + max(Xt5, Yt5, Zt5) + max(Xt6, Yt6, Zt6); i Yt i Zt i And i = 1, 2, ..., 6; obtain the duration of the planned motion segments in the X, Y, and Z directions respectively;

[0102] In this embodiment, the X-direction times are t1=0.83s, t2=0.67s, t3=0.4s, t4=0.67s, t5=0.5s, and t6=0.83s; the Y-direction times are t1=0.8s, t2=0.75s, t3=0.3s, t4=0.75s, t5=0.6s, and t6=0.8s; and the Z-direction times are t1=0.75s, t2=0.67s, t3=0.2s, t4=0.67s, t5=0.5s, and t6=0.75s. The maximum values ​​of the duration of each motion segment in each direction in the global time are taken as follows: t1=0.83s, t2=0.75s, t3=0.4s, t4=0.75s, t5=0.6s, and t6=0.83s. Therefore, the global collaborative planning time is t7=4.16s.

[0103] Step 2.4: Dynamically calculate the actual jerk J based on the jerk J. （实际）;

[0104] Since the global collaborative planning time t is the maximum value of the corresponding motion segment time in each direction, the theoretically calculated displacement S （计算） The actual displacement S is greater than or equal to the actual displacement S, therefore the jerk J needs to be dynamically adjusted to ensure the accuracy of the actual displacement S;

[0105] Theoretical calculation of displacement S （计算） =S1+S2+S3+S4+S5+S6;

[0106] Using the formula: J （实际） =J×[S / S （计算） The jerk J is dynamically adjusted to ensure that the actual displacement S in each direction is consistent with the theoretically calculated displacement.

[0107] Step 2.5: PVT interpolation and velocity calibration;

[0108] Step 2.51: Divide the entire linear motion trajectory into N discrete planning points;

[0109] Specifically, based on the global collaborative planning time t7, and according to the preset control period T, in this embodiment T=1ms, the entire motion process is divided into N discrete planning points, and N=t7 / T;

[0110] Step 2.52: Establish the position sequence P(n) of each axis joint corresponding to N discrete planning points, n=1, 2, ..., N;

[0111] Specifically, by performing an inverse kinematics algorithm on the bidirectional model transformation established in step 1.2, the three-dimensional coordinates (X, Y, F, Z) of the end effector at each planning point are obtained. n Y n Z n ) is converted into the joint target values ​​(T1) of each axis joint of the multi-link manipulator. n T2 n T5 n ), forming the position sequence P(n) of each axis (n=1, 2, ..., N);

[0112] Step 2.53: Construct a position-time and velocity-time relationship model using cubic polynomial PVT interpolation to ensure the continuity of position and velocity between adjacent planning points;

[0113] Specifically, for any two adjacent planning points P(n) (corresponding to time T(n)) and P(n+1) (corresponding to time T(n+1)), a position-time relationship model P(t) is constructed using a cubic polynomial, and the position-time relationship model P(t) satisfies the following equation:

[0114] P(t=at)3 +bt 2 +ct+d(t∈[T(n),T(n+1)]);

[0115] In the formula, a, b, c, and d are all coefficients. The position-time relationship model is set to satisfy the following boundary conditions: P(T(n))=P(n), P(T(n+1))=P(n+1), V(T(n))=V(n), V(T(n+1))=V(n+1), and the initial velocity V(1)=0 and the final velocity V(N)=0.

[0116] Substituting the above boundary conditions into the position-time relationship model, and taking the first derivative of the position-time relationship model with respect to time (V(t)=3at) 2 +2bt+c), forming a system of four linear equations, and solving for the coefficients a, b, c, d through matrix operations, yields the continuous trajectory expression between adjacent planning points, i.e., the position-time relationship model P(t);

[0117] Then, a rough velocity estimate is made for the planning points;

[0118] Specifically, the coarse velocity of the intermediate planning point is calculated using the central difference method. The positional relationship of three adjacent planning points is used to eliminate the single sampling error. For the nth planning point, and 2≤n≤N-1, the formula for calculating its coarse velocity V0(n) satisfies:

[0119] V0(n) = [P(n+1) - P(n-1)] / (2T);

[0120] Set the approximate velocities of the starting and ending planning points to 0, i.e., V0(1)=0, V0(n)=0, to ensure smooth start and stop of motion;

[0121] Based on the position sequence P(n) and the coarse velocity V0(n), a velocity-time relationship model V(t) is constructed using a cubic polynomial, so that the fitted velocity curve satisfies the following: the velocity value at the planning point matches the requirements of the subsequent motion segment, and the first derivative (acceleration) of the velocity curve is continuous.

[0122] Finally, the precise velocity V(n) and position P(n) of each planning point T(n) are output, forming a complete PVT data set of "position P(n)-velocity V(n)-time T(n)".

[0123] Step 3: Control signal output;

[0124] The motion command for each cycle is output to the axis driver via an industrial bus (such as EtherCAT or Profinet). The motion command includes the planned point position P, the precise speed V, and the control cycle T corresponding to that cycle, so as to control the axes of the multi-link robot to move in coordination according to the planned linear motion trajectory.

[0125] Step 4: Interlocking the multi-link robot with external equipment;

[0126] Specifically, the safe zone of the multi-link manipulator is first defined in a three-dimensional coordinate system.

[0127] Then, the three-dimensional coordinates of the end effector of the multi-link robot are collected in real time. If the current coordinates of the end effector are within the safe area, the external device can move freely.

[0128] If the current coordinates are exceeded, the coordinates of each workstation will be compared and traversed. External devices at workstations close to the current location will be locked, while external devices at other workstations will be able to move freely.

[0129] Finally, the above description is only a preferred embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any equivalent substitutions or modifications made by those skilled in the art within the scope of the technology disclosed in the present invention, based on the technical solution and inventive concept of the present invention, should be covered within the scope of protection of the present invention.

Claims

1. A trajectory planning method for a multi-link manipulator based on three-dimensional spatial coordinates, characterized in that, Includes the following steps: Step 1: Establish a bidirectional conversion model between the polar coordinates of each axis joint of the multi-link manipulator and the three-dimensional spatial coordinates of the end effector; Step 2: Based on the three-dimensional coordinates of the starting and ending points of the multi-link manipulator's end effector, a linear motion trajectory is generated by combining 7-segment S-shaped velocity-time curve planning with PVT interpolation, and the position-velocity-time PVT data set of each planned point in the linear motion trajectory is output. Step 3: The PVT data group is synchronously output to the joint drivers of each axis of the multi-link manipulator via the industrial bus, so as to control the joints of the multi-link manipulator to move in coordination according to the planned trajectory. Step 4: Based on the comparison results between the real-time three-dimensional coordinates of the end effector and the preset safety zone, construct a two-way interlock mechanism between the robot and the external equipment.

2. The method for planning the trajectory of a multi-link manipulator based on three-dimensional spatial coordinates according to claim 1, characterized in that, The construction process of the weekend conversion model described in step 1 includes: Position signals are collected by absolute encoders installed at the drive ends of each axis, and combined with axis reduction ratio and transmission efficiency parameters to convert them into joint values ​​for each axis as polar coordinates of each axis joint. The joint values ​​include: joint angles of the rotating axis joints of each arm of the multi-link robot and vertical height values ​​of the lifting axis. A bidirectional conversion model between the polar coordinates of each axis joint and the three-dimensional coordinates of the end effector space is constructed using forward kinematics.

3. The method for planning the trajectory of a multi-link manipulator based on three-dimensional spatial coordinates according to claim 1, characterized in that, The generation of the linear motion trajectory in step 2 includes the following: Step 2.1: Calculate the actual displacement S; The motion start coordinates (X1, Y1, Z1) and end coordinates (X2, Y2, Z2) of the end effector are obtained by a bidirectional transformation model. The displacements ΔX=|X2-X1|, ΔY=|Y2-Y1|, and ΔZ=|Z2-Z1| in each dimension of the three-dimensional coordinate system are calculated to form the actual displacement S=(ΔX, ΔY, ΔZ). Step 2.2: Setting motion constraint parameters; Independent motion constraint parameters are set for each of the X, Y, and Z directions in the three-dimensional spatial coordinate system. The motion constraint parameters include: maximum velocity V, maximum jerk A, and jerk J. Step 2.3: Motion time-displacement planning; The following are calculated for each of the X, Y, and Z directions in three-dimensional spatial coordinates: the duration t1 and displacement S1 of the acceleration segment, the duration t2 and displacement S2 of the uniform acceleration segment, the duration t3 and displacement S3 of the uniform speed segment, the duration t4 and displacement S4 of the deceleration segment, the duration t5 and displacement S5 of the uniform deceleration segment, and the duration t6 and displacement S6 of the deceleration-deceleration segment. The sum of the maximum values ​​of the corresponding motion segment times in each direction is taken as the global collaborative planning time t7. Step 2.4: Dynamically calculate the actual jerk J based on the jerk J. （实际） ; Displacement S planned based on motion segments in each direction （计算） According to the conversion factor K=S / S （计算） Update actual jerk J （实际） =J×K, where S （计算） =S1+S2+S3+S4+S5+S6; Step 2.5: PVT interpolation and velocity calibration; The linear motion trajectory is divided into N planning points according to the set control period T. By performing inverse kinematics on the bidirectional conversion model, the polar coordinates of each axis joint corresponding to the three-dimensional coordinates of the end effector at each planning point are obtained, and a position sequence P(n) is established, n=1,2,…,N. The position-time and velocity-time relationship model is constructed using cubic polynomial PVT interpolation. Finally, the precise velocity V(n) and position P(n) of each planning point T(n) are output, forming a complete PVT data set.

4. The method for planning the trajectory of a multi-link manipulator based on three-dimensional spatial coordinates according to claim 3, characterized in that, In step 2.3, the duration and displacement of each motion segment satisfy the following: The duration of the acceleration segment is t1 = A / J, and the displacement of the acceleration segment is S1 = A³ / (6J²). Duration t2 of uniform acceleration segment: If A 2 If / (2J)≥V, then t2=0; otherwise The displacement of the uniformly accelerated segment is S2 = A × t1 × t2 + (1 / 2) × A × t2 2 ； Duration of the uniform velocity segment t3: If 2×(S1+S2)≥S, then t3=0; otherwise, t3=(S-2×(S1+S2)) / V. Displacement of the uniform acceleration segment S3=t3×V. The duration of the deceleration / acceleration phase is t4=t1, and the displacement of the deceleration / acceleration phase is S4=S1. The duration of the uniform deceleration segment is t5 = t2, and the displacement of the uniform deceleration segment is S5 = S2. The duration of the deceleration / acceleration phase is t6=t1, and the displacement of the deceleration / acceleration phase is S6=S1.

5. The method for planning the trajectory of a multi-link manipulator based on three-dimensional spatial coordinates according to claim 3, characterized in that, The location-time relationship model in step 2.5 is constructed as follows: First, for any two adjacent planning points P(n) and P(n+1), a position-time relationship model P(t) is constructed using a cubic polynomial. The position-time relationship model P(t) satisfies the following equation: P(t)=at 3 +bt 2 +ct+d(t∈[T(n),T(n+1)]); In the formula, a, b, c, and d are all coefficients; Then, the position-time relationship model is set to satisfy the following boundary conditions: P(T(n))=P(n), P(T(n+1))=P(n+1), V(T(n))=V(n), V(T(n+1))=V(n+1), and the initial velocity V(1)=0, and the final velocity V(N)=0; Substitute the above boundary conditions into the position-time relationship model, and take the first derivative of the position-time relationship model with respect to time: V(t)=3at²+2bt+c, forming a system of four linear equations. Solve for the coefficients a, b, c, and d through matrix operations, and finally obtain the position-time relationship model P(t). The speed-time relationship model is constructed as follows: First, the coarse velocity of the intermediate planning point is calculated using the central difference method. For the nth planning point, where 2≤n≤N-1, the formula for calculating its coarse velocity V0(n) satisfies: V0(n) = [P(n+1) - P(n-1)] / (2T); Set the coarse velocity V0(1) = 0 at the starting planning point and the coarse velocity V0(n) = 0 at the ending planning point; Then, based on the position sequence P(n) and the coarse velocity V0(n), a velocity-time relationship model V(t) is constructed using a cubic polynomial.

6. The method for planning the trajectory of a multi-link manipulator based on three-dimensional spatial coordinates according to claim 1, characterized in that, The bidirectional interlocking mechanism described in step 4 is as follows: First, define the safe zone of the multi-link manipulator in a three-dimensional coordinate system. Then, the three-dimensional coordinates of the end effector of the multi-link robot are collected in real time. If the current coordinates of the end effector are within the safe area, the external device can move freely. If the current coordinates are exceeded, the coordinates of each workstation will be compared and traversed. External devices at workstations close to the current location will be locked, while external devices at other workstations will be able to move freely.

Images