A method for determining the workspace of a palletizing robot

Abstract

The application belongs to the field of mechanical arm, and discloses a working space determination method of a stacking robot, and proposes a calculation method of the end motion range of a mechanical arm during joint motion and Cartesian motion. For a stacking robot with the same configuration, the joint angle and arm length are given, and the corresponding joint and end motion range can be calculated. The method is simple in calculation, time-saving, easy to program, and considers the constraints of the mechanical structure, thereby ensuring the safety of people and equipment. Then, the change of the motion range caused by the end gripping goods is calculated. Finally, based on the solved motion range, a torus composed of two concentric circles at a certain height is calculated, the intersection of the torus and the conical region determined by the first joint of the mechanical arm is obtained, and then all the heights are traversed to obtain the working space of the whole stacking robot.

Description

Technical Field

[0001] This invention belongs to the field of robotic arms, specifically relating to a method for determining the workspace of a palletizing robot. Background Technology

[0002] In robotic arm controllers, there are two motion modes: articulated motion and Cartesian motion. Articulated motion involves the individual movement of motors on each joint of the robotic arm, aimed at adjusting the position of a specific joint. Cartesian motion involves the coordinated movement of all joints of the robotic arm, enabling the end effector to move along the X, Y, and Z axes in a base coordinate system, also aimed at adjusting the position of the end effector. In industrial production, controlling robotic arms to complete tasks often involves alternating between these two modes. Regardless of the mode, it is necessary to determine the range of motion of the robotic arm under that mode, and to ensure that the range of motion remains unchanged when switching between the two modes. Furthermore, the range of motion of the end effector differs when it is holding goods compared to when it is not. In addition, due to the constraints between the joints of a palletizing robotic arm, the range of motion of each joint is limited by the mechanical structure. When determining the range of motion in the robotic arm controller, this constraint must be considered; otherwise, it may damage the robotic arm structure and pose safety hazards. To protect the safety of personnel and equipment and ensure the normal operation of production, it is necessary to determine the range of motion of the joints and end effector during articulated motion and Cartesian motion. This range of motion is a subset of the robotic arm's workspace.

[0003] The workspace of a robotic arm refers to the range of motion of its end effector. The workspace of a robotic arm is important for consideration in selecting application scenarios, path planning, production line layout and optimization, and operational safety.

[0004] There are three common methods for determining the workspace of a robotic arm: The first is the analytical method, which involves solving the equation of the outer boundary envelope of the workspace to represent it using algebraic equations. This method requires complex mathematical derivations and is difficult to apply in engineering practice. The second method is the geometric method, which uses the robot's structural parameters to draw the cross-section of the robot's workspace. This method is not suitable for robots with complex structures. The third method is the numerical method, which utilizes the computing power of a computer to map the joint angles of the robotic arm to the position of the end effector using the forward kinematics of the robotic arm. When a sufficient number of joint angles are selected, the coordinate values ​​of the corresponding end effector can accurately reflect the shape of the robotic arm's workspace. This method requires high computational resources, and the accuracy of the workspace obtained depends on the number of joint angles selected.

[0005] Current workspace solutions rarely consider the case where the robotic arm has mechanical constraints, in which case the movements between joints are not independent. Furthermore, existing computational methods, such as numerical methods, are not suitable for engineering implementation with limited computational resources. Finally, most current workspace solutions focus on cases where the end effector does not hold an object. Summary of the Invention

[0006] The purpose of this invention is to provide a method for determining the workspace of a palletizing robot. It proposes a method for calculating the motion range of the robotic arm's end effector during joint and Cartesian motions. For palletizing robots of the same configuration, given the joint angles and arm length, the corresponding motion range of the joints and end effector can be calculated. This method is simple, time-saving, and easy to program, while also considering the constraints of the mechanical structure to ensure the safety of both personnel and equipment. The method then calculates the change in motion range caused by the end effector holding goods. Finally, based on the solved motion range, a ring formed by two concentric circles at a certain height of the end effector is calculated. The intersection of this ring and the conical region defined by the first joint of the robotic arm is taken, and then all heights are traversed to obtain the entire workspace of the robotic arm.

[0007] To achieve the above objectives, the technical solution adopted by the present invention is as follows:

[0008] A method for determining the workspace of a palletizing robot, wherein the palletizing robot has five rotational degrees of freedom, with one rotational degree of freedom each for the first, second, third, and fourth joints, and an auxiliary joint providing one rotational degree of freedom to keep the axis of the fourth joint perpendicular to the ground. A tripod is installed at the third joint to connect two links and form two locally closed chains with the upper arm and forearm. The method for determining the workspace of the palletizing robot includes:

[0009] Establish the coordinate system of the joints and determine the minimum and maximum angles of each joint under unconstrained conditions;

[0010] When the motion mode is joint motion, the range of motion of each joint under geometric relationships and mechanical constraints is determined one by one as the workspace of the robotic arm.

[0011] When the motion mode is Cartesian motion mode, first determine the range of motion of the end effector of the robotic arm in the first joint coordinate system, then convert the range of motion in the first joint coordinate system into the range of motion in the base coordinate system, and generate the workspace of the robotic arm based on the range of motion of the end effector of the robotic arm in the base coordinate system. The end effector of the robotic arm is a point on the axis of the fourth joint, and the first joint is set on the base.

[0012] This invention proposes a method for calculating the motion range and workspace of a palletizing robot arm with a specific configuration, considering mechanical structural constraints. For any palletizing robot arm conforming to this configuration, given the arm length and upper and lower limits of the joint angles, the motion range of the robot arm in both joint mode and Cartesian mode can be calculated. This ensures consistency of the workspace when switching between these two modes, preventing a situation where an area previously belonging to the workspace becomes outside of it after switching from one mode to another. Furthermore, based on the calculated motion range, this invention employs a layered approach to obtain the robot arm's workspace. Specifically, two concentric circles parallel to the XY plane are used to define the motion boundary at a certain height z. This boundary is then intersected with the conical region defined by the first joint of the robot arm. This process is repeated for all possible heights to obtain the entire workspace. Finally, the proposed method is improved to obtain the robot arm's motion range and workspace when the end effector is holding goods. This method is simple to calculate, time-saving, and easy to program. Attached Figure Description

[0013] Figure 1 A schematic diagram illustrating the structure of a four-joint palletizing robot arm, as exemplified in an embodiment of the present invention;

[0014] Figure 2 This is a flowchart of the method for determining the workspace of the palletizing robot of the present invention;

[0015] Figure 3 This invention is based on Figure 1 A model diagram of a four-joint palletizing robot arm;

[0016] Figure 4 This is a schematic diagram illustrating the calculation of angle m in this invention;

[0017] Figure 5 This is a schematic diagram showing the angle range of the third joint under the constraint of the second joint in this invention;

[0018] Figure 6 This is a schematic diagram of the angle range of the second joint under the constraint of the third joint in this invention;

[0019] Figure 7 A flowchart for determining the workspace under the joint movement mode or Cartesian movement mode of this invention;

[0020] Figure 8 This is a flowchart illustrating the calculation of angular and distance offsets caused by the end effector of the robotic arm gripping a cargo.

[0021] Figure 9 This is a schematic diagram of the six-segment circular arc of the present invention;

[0022] Figure 10 This is a schematic diagram illustrating the calculation of the function inarea(x,y) of this invention;

[0023] Figure 11 This is a schematic diagram illustrating the division of the working area into seven regions according to the present invention;

[0024] Figure 12 This is a schematic diagram of two concentric circles in the Z = z1 plane under the base coordinate system frame0 of the present invention;

[0025] Figure 13 This is a schematic diagram illustrating the division of the working area into six regions according to the present invention;

[0026] Figure 14 This is a schematic diagram showing the points corresponding to the maximum and minimum Z1 coordinates in the workspace obtained under the first joint coordinate system frame1 of the present invention.

[0027] Figure 15 This is a flowchart illustrating the entire workspace of the robotic arm in this invention.

[0028] Figure 16 This is a schematic diagram of the workspace of the robotic arm obtained by the present invention;

[0029] Figure 17 This is a schematic diagram of the present invention when the end effector of the robotic arm is holding a cargo. Detailed Implementation

[0030] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0031] Unless otherwise defined, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this invention pertains. The terminology used herein in the description of the invention is for the purpose of describing particular embodiments only and is not intended to limit the invention.

[0032] by Figure 1The four-joint palletizing robot shown is used as an example to illustrate the concept. This four-joint palletizing robot includes a base, a main arm, a forearm, a first joint (also called joint 1), a second joint (also called joint 2), a third joint (also called joint 3), a fourth joint (also called joint 4), and auxiliary joints. The first joint is mounted on the base and connected to a frustum. The main arm is connected to the frustum via the second joint, and the main arm is connected to the forearm via the third joint. The forearm is connected to the fourth joint via the auxiliary joint. This four-joint palletizing robot has five rotational degrees of freedom. The first, second, third, and fourth joints each provide one rotational degree of freedom, and the auxiliary joint provides one rotational degree of freedom to keep the axis of the fourth joint perpendicular to the ground. Taking the end of the robot arm as a point on the axis of the fourth joint, a tripod is installed at the third joint to connect two links and form two locally closed chains with the main arm and forearm.

[0033] like Figure 2 As shown, the workspace determination method for the palletizing robot in this embodiment includes:

[0034] Step 1: Establish the coordinate system of the joints and determine the minimum and maximum angles of each joint under unconstrained conditions.

[0035] First, create models of the base, the upper arm, and the end effector of the robotic arm, such as... Figure 3 As shown, a first joint coordinate system frame1 is established, fixed to the first joint. The origin of the first joint coordinate system frame1 is at the intersection of the first joint axis and the second joint axis. The Z1 axis runs upward along the first joint axis, and the Y1 axis runs in the direction pointing towards the second joint along the second joint axis. The X1 axis is determined by the cross product of the Y1 axis and the Z1 axis. When the rotation angle of the first joint is 0, the base coordinate system frame0 of the base coincides with the first joint coordinate system frame1. The three axes of the base coordinate system frame0 are X, Y, and Z.

[0036] Then, determine the minimum and maximum angles of each joint under unconstrained conditions, including:

[0037] Draw a perpendicular line from the end of the robotic arm to the axis of the auxiliary joint, intersecting the axis of the auxiliary joint. The length of the perpendicular line is l = 144.7550 mm. Define the angle between the perpendicular line l and the XY plane as β. Determine the value of the offset bias1 as bias1 = 100 mm by projecting the position of the end of the robotic arm onto the Y1 axis. Since the axis of the fourth joint is always perpendicular to the XY plane, β = 30.2849° remains constant during the movement of the robotic arm.

[0038] The lengths of the upper arm and the forearm are L1 = 500 mm and L2 = 550 mm respectively. The distances bias2 and bias3 are determined by the distance from the auxiliary joint to the end of the robotic arm. During the movement of the robotic arm, bias2 = l * cos(β) = 125 mm and bias3 = l * sin(β) = 73 mm remain unchanged all the time.

[0039] As Figure 4 shown, calculate the angles determined by the tripod atan is the arctangent function. Let a be a point on the joint axis connecting the link near the forearm and the tripod. Draw a perpendicular line u from point a to the third joint axis, intersecting the third joint axis at point b. Draw a perpendicular line ac from point a to the X1 - Y1 plane, and draw a perpendicular line bc from point b to the Y1 - Z1 plane. The perpendicular lines ac and bc intersect at point c. Then the distance h = ac, and the distance d = bc. In this embodiment, the calculated

[0040] Set theta1, theta2, theta3, and theta4 as the rotation angles of the first joint, the second joint, the third joint, and the fourth joint respectively. The value ranges of the rotation angles of each joint without constraints are as follows: -180° < theta1 < 180°, -180° < theta2 < 0°, 0° < theta3 < 180°, -270° < theta4 < 270°. This range can be set artificially.

[0041] Define joint1 min and joint1 max as the minimum angle and the maximum angle of the first joint without constraints. According to the value range of the rotation angle of the joint without constraints, the value ranges of the minimum angle and the maximum angle of the first joint without constraints are determined as follows: -180° < joint1 min < -90° and 90° < joint1 max < 180°.

[0042] Define joint2 min and joint2 max as the minimum angle and the maximum angle of the second joint without constraints. According to the value range of the rotation angle of the joint without constraints, the value ranges of the minimum angle and the maximum angle of the second joint without constraints are determined as follows: -180° < joint2 min < -90° and -90° < joint2 max < 0°.

[0043] Define joint3 min and joint3 maxTo determine the minimum and maximum angles of the third joint under unconstrained conditions, based on the range of joint rotation angles under unconstrained conditions, the ranges for the minimum and maximum angles of the third joint under unconstrained conditions are as follows: 0° <joint3 min <90° and 90° <joint3 max <180°.

[0044] Define joint3 bias1 joint3 bias2 The angle offset of the third joint satisfies the following constraint: joint3 bias1 =joint3 min and joint3 bias2 +joint3 max =180°. The angle offset can avoid the mechanism deformation problem caused by the parallelogram closed chain degenerating into a straight line when the third joint moves to the minimum and maximum positions.

[0045] Define joint4 min and joint4 max To determine the minimum and maximum angles of the fourth joint under unconstrained conditions, based on the range of joint rotation angles under unconstrained conditions, the ranges for the minimum and maximum angles of the fourth joint under unconstrained conditions are as follows: -270° <joint4 min <0° and 0° <joint4 max <270°.

[0046] It should be noted that the minimum and maximum angles of each joint under unconstrained conditions are based on the robotic arm structure and are manually set according to actual conditions. During workspace calculations, the minimum and maximum angles of each joint under unconstrained conditions remain unchanged. For example, in this embodiment, joint1 is defined as... min = -120° and joint1 max =120° is the minimum and maximum angle of joint 1, define joint2. min = -120° and joint2 max =-10° is the minimum and maximum angle of joint 2, define joint3. min =4.9981° and joint3 max =170° is the minimum and maximum angle of joint 3, defined as joint3. bias1 =4.9981°, joint3 bias2 =10° is the angular offset of joint 3. This offset can prevent the parallelogram closed chain from degenerating into a straight line when joint 3 moves to its minimum and maximum positions, thus avoiding the mechanism deformation problem. Define joint4. min =-200° and joint4max =200° is the minimum and maximum angle of joint 4. Step 2: When the motion mode is joint motion, determine the range of motion of each joint under geometric relationships and mechanical constraints as the workspace of the robotic arm.

[0047] This invention uses angle m to divide the joint angle range to solve the mechanical structure constraint problem. Because of the constraint of the local closed chain, angle m remains constant throughout the robot arm's movement. Figure 5 and Figure 6 As shown, combining the geometric relationships and mechanical constraints of the four-joint palletizing robot arm, the minimum angle of the third joint under the constraints is obtained as max(joint3). bias1 ,-theta2-m+joint3 bias1 Under constraints, the maximum angle of the third joint is min(joint3). max ,180°-m-joint3 bias2 -theta2), the minimum angle of the second joint under the constraint is max(joint2). min ,-theta3-m+joint3 bias1 Under constraints, the maximum angle of the second joint is min(joint2). max ,180°-m-joint3 bias2 -theta3).

[0048] like Figure 7 As shown, the range of motion of the first joint under constraints is determined as follows:

[0049] theta1∈[joint1 min joint1 max ]

[0050] The range of motion of the second joint under constraints is determined as follows:

[0051] theta2∈[theta2 min (theta3),theta2 max (theta3)]

[0052] theta2 min (theta3) = max(joint2) min ,-theta3-m+joint3 bias1 )

[0053] theta2 max (theta3)=min(joint2 max ,180°-m-joint3 bias2 -theta3)

[0054] In the formula, theta2 min (theta3) is the minimum angle of the second joint under the constraint of the third joint, and theta2 is... max (theta3) is the maximum angle of the second joint under the constraint of the third joint. In this embodiment, theta2 min (theta3)=max(-120,-theta3-50), theta2 max (theta3)=min(-10,115.0019-theta3).

[0055] The range of motion of the third joint under constraints is determined as follows:

[0056] theta3∈[theta3 min (theta2),theta3 max (theta2)]

[0057] theta3 min (theta2) = max(joint3) bias1 ,-theta2-m+joint3 bias1 )

[0058] theta3 max (theta2)=min(joint3 max ,180°-m-joint3 bias2 -theta2)

[0059] In the formula, theta3 min (theta2) is the minimum angle of the third joint under the constraint of the second joint, and theta3 is... max (theta2) is the maximum angle of the third joint under the constraint of the second joint. In this embodiment, theta3 min (theta2)=max(4.9981,-theta2-50),theta3 max (theta2)=min(170,115.0019-theta2).

[0060] The range of motion of the fourth joint under constraints is determined as follows:

[0061] theta4∈[joint4 min joint4 max ]

[0062] In the joint movement mode, if any of the joint angles theta1, theta2, theta3, and theta4 exceeds the corresponding range of motion, the robotic arm will be outside the workspace.

[0063] If the current angles of each joint are theta1 = 0°, theta2 = -90°, theta3 = 90° and theta4 = 0°, then the range of motion of each joint is theta1 ∈ [-120°, 120°], theta2 ∈ [-120°, -10°], theta3 ∈ [40°, 170°] and theta4 ∈ [-200°, 200°].

[0064] When the robotic arm's end effector grips a cargo, the range of motion and workspace of the robotic arm will change, requiring adjustments to the workspace. The adjustment process for determining the workspace under articulated motion is as follows:

[0065] When the robotic arm's end effector grips a cargo, a diameter that can accommodate the cargo is selected as... For example, a sphere with w = 100 has a circular cross-section. M points (e.g., M = 50) are uniformly selected on the circle. The coordinates of these M points in the X1-Z1 plane of the first joint coordinate system frame1 are:

[0066]

[0067] Iterate through each point in the set s(x1,z1,w,M), and count the number of points that make the incart(p2) function equal to 0, denoted as n(s(x1,z1,w,M)). Predetermine a positive integer as the threshold threshold, threshold ≤ M (e.g., threshold = 10). The formula for the incart(p2) function is:

[0068] incart(p2)=(1-incircle(p2,C2,r2))*(1-incircle(p2,C4,r4))*(1-incircle(p2,C5,r5))*

[0069] (incircle(p2,C1,r1)+incircle(p2,C3,r3)+incircle(p2,C6,r6)),p2∈s(x1,z1,w,M)

[0070] When the motion mode is joint motion, if n(s(x1,z1,w,M))>thres, then the robotic arm is not in the workspace and the process ends; otherwise, update the motion range of each joint under the constraints as follows:

[0071] theta1∈[joint1min joint1 max ]

[0072] theta2∈[theta2 min (theta3)+offset min2 theta2 max (theta3)-offset max2 ]

[0073] in:

[0074] theta2 min (theta3) = max(joint2) min ,-theta3-m+joint3 bias1 )

[0075] theta2 max (theta3)=min(joint2 max ,180°-m-joint3 bias2 -theta3)

[0076] theta3∈[theta3 min (theta2)+offset min3 theta3 max (theta2)-offset max3 ]

[0077] in:

[0078] theta3 min (theta2) = max(joint3) bias1 ,-theta2-m+joint3 bias1 )

[0079] theta3 max (theta2)=min(joint3 max ,180°-m-joint3 bias2 -theta2)

[0080] theta4∈[joint4 min joint4 max ]

[0081] In the above formula, offset min2 offset max2 offset min3 and offset max3 The angular offset caused by the end effector gripping the goods, such as Figure 8 As shown, the solution is obtained according to the following procedure:

[0082] (1) Initialize the iteration start point v0, the coordinates (x0, z0) of the end of the robotic arm in the first joint coordinate system frame1 X1-Z1 plane, the step size of each iteration step, the threshold threshold, the maximum number of iterations maxn, define the x0 coordinate update formula update_x0 and the z0 coordinate update formula update_z0, and take the parameter v1 = 0 and the parameter v2 = step.

[0083] When the parameter to be solved is offset min2 At that time, v0, x0, z0, update_x0, and update_z0 are initialized as follows:

[0084] v0 = theta2 min (theta3)

[0085] x0=x(theta2 min (theta3),theta3)

[0086] z0=z(theta2 min (theta3),theta3)

[0087] update_x0 = x(v0 + v1, theta3)

[0088] update_z0 = z(v0 + v1, theta3)

[0089] When the parameter to be solved is offset max2 At that time, v0, x0, z0, update_x0, and update_z0 are initialized as follows:

[0090] v0 = theta2 max (theta3)

[0091] x0=x(theta2 max (theta3),theta3)

[0092] z0=z(theta2 max (theta3),theta3)

[0093] update_x0 = x(v0-v1, theta3)

[0094] update_z0 = z(v0-v1, theta3)

[0095] When the parameter to be solved is offset min3At that time, v0, x0, z0, update_x0, and update_z0 are initialized as follows:

[0096] v0 = theta3 min (theta2)

[0097] x0 = x(theta2,theta3) min (theta2))

[0098] z0 = z(theta2,theta3) min (theta2))

[0099] update_x0 = x(theta2, v0 + v1)

[0100] update_z0 = z(theta2, v0 + v1)

[0101] When the parameter to be solved is offset max3 At that time, v0, x0, z0, update_x0, and update_z0 are initialized as follows:

[0102] v0 = theta3 max (theta2)

[0103] x0 = x(theta2,theta3) max (theta2))

[0104] z0 = z(theta2,theta3) max (theta2))

[0105] update_x0 = x(theta2, v0 - v1)

[0106] update_z0 = z(theta2, v0 - v1)

[0107] (2) Set the current iteration count cnts = 0, the parameter curl = n(s(x0,z0,w,M)), traverse each point in the set s(x0,z0,w,M), and take the number of points that make the value of the incart(p3) function 0, denoted as n(s(x0,z0,w,M)). The set s(x0,z0,w,M) and the expression of the incart(p3) function are as follows:

[0108]

[0109] incart(p3)=(1-incircle(p3,C2,r2))*(1-incircle(p3,C4,r4))*(1-incircle(p3,C5,r5))*

[0110] (incircle(p3,C1,r1)+incircle(p3,C3,r3)+incircle(p3,C6,r6)),p3∈s(x0,z0,w,M)

[0111] (3) If cnts < maxn, then execute step (4); otherwise, output the value of v1 as the value of the parameter to be solved and end.

[0112] (4) If curl≥thres or curl=0, then execute step (5); otherwise, output the value of v1 as the value of the parameter to be solved and end.

[0113] (5) If curl ≥ thres, then execute step (6); otherwise execute step (7).

[0114] (6) The parameters are iteratively updated as follows:

[0115] v2 = v1

[0116] v1 = v1 + step

[0117] step = step * 2

[0118] x0 = update_x0

[0119] z0 = update_z0

[0120] curn=n(s(x0,z0,w,M))

[0121] cnts = cnts + 1

[0122] After the update is complete, return to step (3).

[0123] (7) The parameters are iteratively updated as follows:

[0124] v1 = (v1 + v2) * 0.5

[0125] x0 = update_x0

[0126] z0 = update_z0

[0127] curn=n(s(x0,z0,w,M))

[0128] cnts = cnts + 1

[0129] After the update is complete, return to step (3).

[0130] Given the current theta1 = 30°, theta2 = -110°, theta3 = 90° and theta4 = 0°, x1 = 470.8209, z1 = 584.9574, and n(s(x1,z1,w,n)) = 0, the range of motion of each joint when the end effector of the robotic arm grips the goods is: theta1 ∈ [-120°, 120°], theta2 ∈ [-112.5°, -14.9951°], theta3 ∈ [60°, 165°], and theta4 ∈ [-200°, 200°].

[0131] Step 3: When the motion mode is Cartesian motion mode, first determine the range of motion of the end effector of the robotic arm in the first joint coordinate system, and then convert the range of motion in the first joint coordinate system into the range of motion in the base coordinate system. Generate the workspace of the robotic arm based on the range of motion of the end effector of the robotic arm in the base coordinate system. The end effector of the robotic arm is a point on the axis of the fourth joint, and the first joint is set on the base.

[0132] The position of the robotic arm's end effector is transformed from the base coordinate system to the first joint coordinate system through rotational transformation. The range of motion of the end effector is considered in the first joint coordinate system. The range of motion of the end effector along the Z-axis is equal to the range of motion of the end effector along Z1 in the first joint coordinate system. The range of motion of the end effector along the X and Y axes consists of the intersection of two concentric circles parallel to the XY plane and a conical region defined by the first joint. The radii of these two circles are obtained from the boundary of the end effector in the first joint coordinate system, while the conical region is determined by the maximum and minimum values ​​of the angles of the first joint of the robotic arm. Since this boundary is calculated from the joint motion range in the joint motion mode, the consistency of the workspace can be ensured when switching between the joint motion mode and the Cartesian motion mode; that is, there will be no situation where an area that was originally part of the workspace becomes not part of the workspace after switching from one mode to another.

[0133] The rotation angles theta1, theta2, theta3, and theta4 of each joint are obtained through sensors: theta1 = 30°, theta2 = -110°, theta3 = 90°, and theta4 = 0°. Then, based on the rotation angle values ​​of each joint, the position of the robotic arm end effector in the base coordinate system frame0 is calculated as (x, y, z) using the following formula. T ,

[0134]

[0135] After substituting the values, it becomes:

[0136]

[0137] The position of the robotic arm's end effector in the first joint coordinate system frame1 is then calculated as follows:

[0138]

[0139] After substituting the values, it becomes:

[0140]

[0141] The position of the robotic arm's end effector in the first joint coordinate system frame1 is denoted as p = (x1 bias1 z1). T ,like Figure 9 As shown, with the first joint fixed, calculate six circular arcs, where C1, C2, C3, C4, C5, and C6 are the centers, and their coordinates are as follows:

[0142] C1:(L2*cos(m-joint3 bias1 )+bias2,L2*sin(m-joint3 bias1 )-bias3)

[0143] C2:(L1*cos(-joint2 min )+bias2,L1*sin(-joint2 min )-bias3)

[0144] C3:(bias2,-bias3)

[0145] C4:(bias2,-bias3)

[0146] C5:(L2*cos(180°-m-joint3 bias2 )+bias2,L2*sin(-180°+m+joint3 bias2 )-bias3)

[0147] C6:(L1*cos(-joint2 max )+bias2,L1*sin(-joint2 max )-bias3)

[0148] After substituting the values, we get:

[0149] C1:(478.5332,348.3244)

[0150] C2:(-125,360.0127)

[0151] C3:(125,-73)

[0152] C4:(125,-73)

[0153] C5:(-107.4566,-571.4616)

[0154] C6:(617.4039,13.8241)

[0155] In the formula, C1 is the center of the first arc, C2 is the center of the second arc, C3 is the center of the third arc, C4 is the center of the fourth arc, C5 is the center of the fifth arc, and C6 is the center of the sixth arc.

[0156] The radii of the six arcs are as follows:

[0157] r1=L1

[0158] r2=L2

[0159]

[0160]

[0161] r5 = L1

[0162] r6=L2

[0163] After substituting the values, we get:

[0164] r1 = 500

[0165] r2 = 550

[0166] r3 = 1049.0037

[0167] r4 = 104.1909

[0168] r5 = 500

[0169] r6 = 550

[0170] In the formula, r1 is the radius of the first arc segment, r2 is the radius of the second arc segment, r3 is the radius of the third arc segment, r4 is the radius of the fourth arc segment, r5 is the radius of the fifth arc segment, and r6 is the radius of the sixth arc segment. The area enclosed by the six arc segments is taken as the range of motion of the robotic arm end effector in the first joint coordinate system.

[0171] Define x(theta2,theta3) and z(theta2,theta3) as the coordinates of the robotic arm's end effector in the first joint coordinate system frame1X1-Z1 plane:

[0172] x(theta2,theta3)=L2*cos(theta2+theta3)+L1*cos(theta2)+bias2

[0173] z(theta2,theta3)=-L2*sin(theta2+theta3)-L1*sin(theta2)-bias3

[0174] After substituting the values, we get:

[0175] x(theta2,theta3)=550*cos(theta2+theta3)+500*cos(theta2)+125

[0176] z(theta2,theta3)=-550*sin(theta2+theta3)-500*sin(theta2)-73

[0177] The intersection points p1, p2, p3, p4, p5, p6 of the six circular arcs in the X1-Z1 plane of the first joint coordinate system frame1 are calculated as follows:

[0178] p1:(x(joint2 min theta3 min (joint2 min )),z(joint2 min theta3 min (joint2 min )))

[0179] p2:(x(-m,theta3 min (-m)),z(-m,theta3 min (-m)))

[0180] p3:(x(joint2 max theta3 min (joint2 max )),z(joint2 max theta3 min (joint2 max )))

[0181] p4:(x(joint2 max theta3 max (joint2 max )),z(joint2 max theta3 max (joint2 max ))

[0182] p5:(x(-m,theta3 max (-m)),z(-m,theta3max (-m)))

[0183] p6:(x(joint2 min theta3 max (joint2 min )),z(joint2 min theta3 max (joint2 min )))

[0184] theta3 min (joint2 min = max(joint3) bias1 ,-joint2 min -m+joint3 bias1 )

[0185] theta3 min (-m)=max(joint3 bias1 joint3 bias1 )

[0186] theta3 min (joint2 max = max(joint3) bias1 ,-joint2 max -m+joint3 bias1 )

[0187] theta3 max (joint2 max ) = min(joint3 max ,180°-m-joint3 bias2 -joint2 max )

[0188] theta3 max (-m)=min(joint3 max ,180°-joint3 bias2 )

[0189] theta3 max (joint2 min ) = min(joint3 max ,180°-m-joint3 bias2 -joint2 min )

[0190] After substituting the values, we get:

[0191] p1:(228.5332,781.3371)

[0192] p2:(765.3350,757.8910)

[0193] p3:(1165.3094,61.7779)

[0194] p4:(384.9473,-484.6375)

[0195] p5:(179.3452,-161.8951)

[0196] p6:(228.5332,-61.3117)

[0197] In the formula, theta3 min (joint2 min ( ) represents the angle joint2 min The minimum angle of the third joint under constraint, theta3 min (-m) is the minimum angle of the third joint under the constraint of angle m, theta3 min (joint2 max ( ) represents the angle joint2 max The minimum angle of the third joint under constraint, theta3 max (joint2 max ( ) represents the angle joint2 max The maximum angle of the third joint under constraints, theta3 max (-m) represents the maximum angle of the third joint under the constraint of angle m, theta3 max (joint2 min ( ) represents the angle joint2 min The maximum angle of the third joint under constraint. Figure 9 In the diagram, p1-p6 represent the intersection points p1, p2, p3, p4, p5, p6.

[0198] Define p ix The intersection point p is i = 1, 2, 3, 4, 5, 6. i The x-coordinate, defined as p iz The intersection point p is i = 1, 2, 3, 4, 5, 6. i The ordinates of p are sorted in descending order. ix Arranged as p ixj j = 1, 2, 3, 4, 5, 6, where j represents the order:

[0199] p 3x 1 =1165.3094

[0200] p 2x 2 =765.3350

[0201] p 4x 3 =384.9473

[0202] p 1x 4 =228.5332

[0203] p 6x 5 =228.5332

[0204] p 5x 6 =179.3452

[0205] Sort p in descending order iz Arranged as p izj j = 1, 2, 3, 4, 5, 6, where j represents the order:

[0206] p 1z 1 =781.3371

[0207] p 2z 2 =757.8910

[0208] p 3z 3 =61.7779

[0209] p 6z 4 =-61.3117

[0210] p 5z 5 =-161.8951

[0211] p 4z 6 =-484.6375

[0212] calculate If the value of incart(p) is 2, the process ends; otherwise, execution continues. In this embodiment, incart(p) is 2, so execution continues.

[0213] The general format of the function `incircle` is `incircle(p′,center,r)`, where `center` is the coordinate (x',z') of the center of the circle in the first joint coordinate system `frame1 X1-Z1` plane, `r` is the radius of the circle, and the coordinate `p′` records the coordinates (x″,z″) of the end effector of the robotic arm in the first joint coordinate system `frame1 X1-Z1` plane. If (x'-x″) 2 +(z'-z″) 2 ≤r 2 If the expression is true, the function value is 1; otherwise, the function value is 0.

[0214] The function inarea(x,y) is calculated as follows, defining the equation of line1 as y = tan(joint1). max Let x be the equation of line 2, and let y = tan(joint1).min The lines line1 and line2 enclose a cone-shaped region. When the end effector of the robotic arm is not within the workspace (x, y), i.e., as shown in the image, the situation becomes more complex. Figure 10 As shown, when y < tan(joint1) max )*x and y>tan(joint1 min When x is multiplied by 1, the function value is 1; otherwise, the function value is 0.

[0215] Next, we determine the range of motion of the robotic arm's end effector along the X and Y axes in the base coordinate system frame0, that is, the range of motion of the robotic arm's end effector along the X and Y axes in Cartesian motion mode. The steps are as follows:

[0216] The working area is defined as the range of motion of the robotic arm's end effector in the first joint coordinate system, along the Z1 axis coordinate p. i The work area is divided into seven regions, i = 1, 2, 3, 4, 5, 6, as follows: Figure 11 As shown ( Figure 11 In the diagram, p1-p6 represent the intersection points p1, p2, p3, p4, p5, p6. Traversing p... izj If we can find a way to make z1≥p izj The first p in j = 1, 2, 3, 4, 5, 6 izj If the value is j, then the robotic arm's end effector is in the j-th region; otherwise, it is in the seventh region. Substituting the values, we can see that traversing p... izj Then, find the first p. izj Make 584.9574≥p 3z3 So the end effector of the robotic arm is currently in the third region.

[0217] Then, based on the arc covered by the defined region, calculate the maximum and minimum X1 coordinates of the robot arm's end effector motion in the X1-Z1 plane:

[0218]

[0219]

[0220] After substituting the values, it becomes:

[0221]

[0222]

[0223] Where, r p and r q Let C be the radius of the p-th circle and the q-th circle. px and C qx Let C be the x-coordinate of the center of the p-th circle and the q-th circle. pz and C qzLet x be the z-coordinate of the center of the p-th circle and the q-th circle. l Let x be the maximum X1 coordinate of the robot arm's end effector motion in the X1-Z1 plane. s The minimum X1 coordinate for the end effector motion of the X1-Z1 planar robotic arm.

[0224] Calculate two concentric circles in the Z = z1 plane within the base coordinate system frame0. The annulus formed by the two concentric circles determines the range of motion of the robot arm's end effector at height z1, and also represents a section of the workspace. Figure 12 As shown, the radii of the two concentric circles are:

[0225]

[0226]

[0227] In the formula, r max Let r be the radius of the larger of the two concentric circles. min It is the radius of the smaller of the two concentric circles.

[0228] When joint1 min = -180° and joint1 max When the angle is 180°, determine the range of movement of the robotic arm's end effector along the X-axis:

[0229]

[0230]

[0231] In the formula, X max X is the maximum value of the robotic arm's end effector moving along the X-axis. min This represents the minimum value at which the end effector of the robotic arm moves along the X-axis.

[0232] Determine the range of movement of the robotic arm's end effector along the Y-axis:

[0233]

[0234]

[0235] In the formula, Y max Y is the maximum value of the robotic arm's end effector movement along the Y-axis. min This represents the minimum value at which the end effector of the robotic arm moves along the Y-axis.

[0236] When joint1 min ≠ -180° or joint1 max When the angle is not equal to 180°, first determine the equations of lines line1 and line2:

[0237] line1: y = tan(joint1)max )*x

[0238] line2: y = tan(joint1) min )*x

[0239] After substituting the values, it becomes:

[0240] line1: y = -1.7321*x

[0241] line 2: y = 1.7321 * x

[0242] At this point, the workspace becomes the intersection of a cone-shaped area enclosed by lines 1 and 2 and a circular ring, defining the range of movement of the robotic arm's end effector along the X-axis:

[0243]

[0244]

[0245] Substituting the values, we get:

[0246] X min =219.9055

[0247] X max =890.8901

[0248] Where the function The calculation is as follows, defining the equation of line3 as: Define the equation of line4 as follows: Lines 3 and 4 form a cone-shaped area, when and When the condition is met, the function value is 1; otherwise, the function value is 0.

[0249] Determine the range of movement of the robotic arm's end effector along the Y-axis:

[0250]

[0251]

[0252] Substituting the values, we get:

[0253] Y min =155.1480

[0254] Y max =877.1532

[0255] To determine the range of movement of the robotic arm's end effector along the Z-axis under Cartesian motion, the steps are as follows:

[0256] like Figure 13 As shown ( Figure 13 In the diagram, p1-p6 represent the intersection points p1, p2, p3, p4, p5, p6, along the X1 axis coordinate p i Divide the working area into six regions, i = 1, 2, 3, 4, 5, 6, and iterate through p. ixj Find the value of x1≥p ixj The first p in j = 1, 2, 3, 4, 5, 6 ixj Then the endpoint is currently in the j-th region. After substituting the value, iterate through p. ixj Find the first p ixj Make 470.8209≥p 4x3 Then the end is in the third region at this time.

[0257] Then, based on the arc covered by the defined region, calculate the maximum and minimum Z1 coordinates of the motion within the end of the frame1 X1-Z1 plane in the first joint coordinate system:

[0258]

[0259]

[0260] After substituting the values, it becomes:

[0261]

[0262]

[0263] Where, r p and r q Let C be the radius of the p-th circle and the q-th circle. px and C qx Let C be the x-coordinate of the center of the p-th circle and the q-th circle. pz and C qz Let z be the z-coordinate of the center of the p-th circle and the q-th circle. l Let z be the maximum Z1 coordinate of the robot arm's end effector motion in the X1-Z1 plane. s Let Z1 be the minimum Z1 coordinate of the robot arm's end effector movement in the X1-Z1 plane; when there is more than one solution, how to find the maximum Z1 coordinate of the movement along the Z-axis? max Then take the value that makes |z l -z1| is the minimum solution; if we want to find the minimum value Z that moves along the Z-axis. min Then take the value that makes |z s -z1| is the smallest solution; therefore, Z is obtained. max =z l Z min =z s Substituting the values, we get Z. max =848.2649, Z min= -516.2829.

[0264] Based on the above calculation steps, the workspace of the robotic arm is further obtained as follows:

[0265] The point Z corresponding to the maximum value of Z1 coordinate in the workspace is obtained under the first joint coordinate system frame1. top =(C 1x bias1 C 1z +r1) T (Substitute the value as Z) top =(478.5332100848.3244) T The point Z corresponding to the minimum value. down =(C 6x bias1 C 6z -r6) T (Substitute the value as Z) down = (617.4039100 -536.1759) T ),like Figure 14 As shown. On the line segment Above, N (N=69) points are evenly selected at intervals d (d=20), and the coordinates of each point are labeled P. n =(x n bias1 z n ) T Then for each coordinate P n Calculate the radius r at the corresponding height. min and r max Two concentric circles, parallel to the XY plane of the base coordinate system frame0, and the annular portion enclosed by the two concentric circles and the conical region enclosed by lines line1 and line2 define a portion of the workspace:

[0266]

[0267] After traversing all P n Then, the entire workspace of the robotic arm is obtained, as shown in the flowchart below. Figure 15 As shown, the resulting workspace is as follows Figure 16 As shown. Where, C 1x and C 6x Let C be the x-coordinate of the center of the 1st and 6th circles. 1z and C 6z Let n be the z-coordinate of the center of the 1st and 6th circles, where n∈[1,N].

[0268] like Figure 17 As shown, when the robotic arm's end effector grips a cargo, the robotic arm's range of motion and workspace will change, requiring adjustments to the workspace. The adjustment process is as follows:

[0269] When the robotic arm's end effector grips a cargo, a diameter that can accommodate the cargo is selected as... For example, a sphere with w = 100 has a circular cross-section. M points (e.g., M = 50) are uniformly selected on the circle. The coordinates of these M points in the X1-Z1 plane of the first joint coordinate system frame1 are:

[0270]

[0271] Iterate through each point in the set s(x1,z1,w,M), and count the number of points that make the incart(p2) function equal to 0, denoted as n(s(x1,z1,w,M)). Predetermine a positive integer as the threshold threshold, threshold ≤ M (e.g., threshold = 10). The formula for the incart(p2) function is:

[0272] incart(p2)=(1-incircle(p2,C2,r2))*(1-incircle(p2,C4,r4))*(1-incircle(p2,C5,r5))*

[0273] (incircle(p2,C1,r1)+incircle(p2,C3,r3)+incircle(p2,C6,r6)),p2∈s(x1,z1,w,M)

[0274] When the motion pattern is Cartesian, if n(s(x1,z1,w,M))>thres, then the robotic arm is not in the workspace and the process ends; otherwise, change x. l x s , z l and z s The calculation method is as follows: other calculations are performed according to the method used when the robotic arm's end effector is not gripping any goods, determining the workspace (i.e., only replacing x in the overall process when the robotic arm's end effector is not gripping any goods). l x s , z l and z s The calculation formula remains the same for all other calculation formulas and principles:

[0275]

[0276]

[0277]

[0278]

[0279] Since n(s(x1,z1,w,M)) = 0, substituting the values, we get:

[0280] x l =927.0072

[0281] x s =441.8963

[0282] z l =843.2698

[0283] z s = -511.2878

[0284] Where, r p and r q Let C be the radius of the p-th circle and the q-th circle. px and C qx Let C be the x-coordinate of the center of the p-th circle and the q-th circle. pz and C qz Let x be the z-coordinate of the center of the p-th circle and the q-th circle. l Let x be the maximum X1 coordinate of the robot arm's end effector motion in the X1-Z1 plane. s Let z be the minimum X1 coordinate of the robot arm's end effector motion in the X1-Z1 plane. l Let z be the maximum Z1 coordinate of the robot arm's end effector motion in the X1-Z1 plane. s Let offset be the minimum Z1 coordinate of the robot arm's end effector motion in the X1-Z1 plane. xl ,offset xs ,offset zl and offset zs The distance offset caused by the robotic arm's end effector gripping the goods is solved using the following procedure:

[0285] (1) Initialize the iteration start point v0, the coordinates (x0, z0) of the end of the robotic arm in the first joint coordinate system frame1 X1-Z1 plane, the step size of each iteration step, the threshold threshold, the maximum number of iterations maxn, define the x0 coordinate update formula update_x0 and the z0 coordinate update formula update_z0, and take the parameter v1 = 0 and the parameter v2 = step;

[0286] When the parameter to be solved is offset xl At that time, v0, x0, z0, update_x0, and update_z0 are initialized as follows:

[0287] v0 = x l

[0288] x0=x l

[0289] z0 = z1

[0290] update_x0 = v0 - v1

[0291] update_z0=z1

[0292] When the parameter to be solved is offset xs At that time, v0, x0, z0, update_x0, and update_z0 are initialized as follows:

[0293] v0 = x s

[0294] x0=x s

[0295] z0 = z1

[0296] update_x0 = v0 + v1

[0297] update_z0=z1

[0298] When the parameter to be solved is offset zl At that time, v0, x0, z0, update_x0, and update_z0 are initialized as follows:

[0299] v0 = z l

[0300] x0=x1

[0301] z0=z l

[0302] update_x0=x1

[0303] update_z0 = v0 - v1

[0304] When the parameter to be solved is offset zs At that time, v0, x0, z0, update_x0, and update_z0 are initialized as follows:

[0305] v0 = z s

[0306] x0=x1

[0307] z0=z s

[0308] update_x0=x1

[0309] update_z0 = v0 + v1

[0310] (2) Set the current iteration count cnts = 0, the parameter curl = n(s(x0,z0,w,M)), traverse each point in the set s(x0,z0,w,M), and take the number of points that make the value of the incart(p3) function 0, denoted as n(s(x0,z0,w,M)). The set s(x0,z0,w,M) and the expression of the incart(p3) function are as follows:

[0311]

[0312] incart(p3)=(1-incircle(p3,C2,r2))*(1-incircle(p3,C4,r4))*(1-incircle(p3,C5,r5))*

[0313] (incircle(p3,C1,r1)+incircle(p3,C3,r3)+incircle(p3,C6,r6)),p3∈s(x0,z0,w,M)

[0314] (3) If cnts < maxn, then execute step (4); otherwise, output the value of v1 as the value of the parameter to be solved and end.

[0315] (4) If curl≥thres or curl=0, then execute step (5); otherwise, output the value of v1 as the value of the parameter to be solved and end.

[0316] (5) If curl ≥ thres, then execute step (6); otherwise execute step (7).

[0317] (6) The parameters are iteratively updated as follows:

[0318] v2 = v1

[0319] v1 = v1 + step

[0320] step = step * 2

[0321] x0 = update_x0

[0322] z0 = update_z0

[0323] curn=n(s(x0,z0,w,M))

[0324] cnts = cnts + 1

[0325] After the update is complete, return to step (3).

[0326] (7) The parameters are iteratively updated as follows:

[0327] v1 = (v1 + v2) * 0.5

[0328] x0 = update_x0

[0329] z0 = update_z0

[0330] curn=n(s(x0,z0,w,M))

[0331] cnts = cnts + 1

[0332] After the update is complete, return to step (3).

[0333] The technical features of the above embodiments can be combined in any way. For the sake of brevity, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.

[0334] The embodiments described above are merely illustrative of several implementations of the present invention, and while the descriptions are specific and detailed, they should not be construed as limiting the scope of the invention. It should be noted that those skilled in the art can make various modifications and improvements without departing from the concept of the present invention, and these modifications and improvements all fall within the scope of protection of the present invention. Therefore, the scope of protection of the present invention should be determined by the appended claims.

Claims

1. A method for determining the workspace of a palletizing robot, wherein the palletizing robot has five rotational degrees of freedom, with each of the first, second, third, and fourth joints providing one rotational degree of freedom, an auxiliary joint providing one rotational degree of freedom to keep the axis of the fourth joint always perpendicular to the ground, and a tripod installed at the third joint connecting two links and forming two locally closed chains with the upper arm and forearm, characterized in that, The method for determining the workspace of the palletizing robot includes: Establish the coordinate system of the joints and determine the minimum and maximum angles of each joint under unconstrained conditions; establishing the coordinate system of the joints includes: establishing the coordinate system of the first joint fixed to the first joint. First joint coordinate system The origin is located at the intersection of the first joint axis and the second joint axis, and extends upwards along the first joint axis. The axis, along the axis of the second joint, points in the direction of the second joint. Axis, with Axis cross product The result of the axis is determined Axis; when the rotation angle of the first joint is equal to 0, the base coordinate system of the base. and the first joint coordinate system Coincident, base coordinate system The three axes are ; When the motion mode is joint motion, the range of motion of each joint under geometric relationships and mechanical constraints is determined one by one as the workspace of the palletizing robot. When the motion mode is Cartesian motion mode, first determine the range of motion of the end effector of the robotic arm in the first joint coordinate system, then convert the range of motion in the first joint coordinate system into the range of motion in the base coordinate system, and generate the workspace of the palletizing robot based on the range of motion of the end effector of the robotic arm in the base coordinate system. The end effector of the robotic arm is a point on the axis of the fourth joint, and the first joint is set on the base. The determination of the minimum and maximum angles of each joint under unconstrained conditions includes: Draw a perpendicular line from the end effector of the robotic arm to the axis of the auxiliary joint, intersecting the axis of the auxiliary joint. The length of the perpendicular line is... Define the perpendicular line and The included angle between the planes is By the position of the end of the robotic arm Projection of the axis to determine the offset The value; Take the lengths of the upper arm and forearm respectively and Determined by the distance from the auxiliary joint to the end of the robotic arm and ,Pick and ; Calculate the angle determined by the tripod , It is the arctangent function, take It is a point on the joint axis connecting the link near the forearm and the tripod, passing through point... Draw a perpendicular line to the axis of the third joint. Intersecting the axis of the third joint at point past the point do perpendicular line of a plane past the point do perpendicular line of a plane ,perpendicular and perpendicular line Intersection point Then the distance ,distance ; set up , , and The rotation angles of the first, second, third, and fourth joints are taken from the following ranges under unconstrained conditions: , , , ; definition and To determine the minimum and maximum angles of the first joint under unconstrained conditions, based on the range of joint rotation angles under unconstrained conditions, the ranges for the minimum and maximum angles of the first joint under unconstrained conditions are as follows: and ; definition and To determine the minimum and maximum angles of the second joint under unconstrained conditions, based on the range of joint rotation angles under unconstrained conditions, the ranges for the minimum and maximum angles of the second joint under unconstrained conditions are as follows: and ; definition and To determine the minimum and maximum angles of the third joint under unconstrained conditions, based on the range of joint rotation angles under unconstrained conditions, the ranges for the minimum and maximum angles of the third joint under unconstrained conditions are as follows: and ; definition The angle offset of the third joint satisfies the following constraints: and ; definition and To determine the minimum and maximum angles of the fourth joint under unconstrained conditions, based on the range of joint rotation angles under unconstrained conditions, the ranges for the minimum and maximum angles of the fourth joint under unconstrained conditions are as follows: and ; When the motion mode is joint motion, the range of motion of each joint under geometric relationships and mechanical constraints is determined as the workspace of the palletizing robot, including: Combining the geometric relationships and mechanical constraints of the palletizing robot, the minimum angle of the third joint under the constraints is obtained as follows: The maximum angle of the third joint under constraint is The minimum angle of the second joint under constraint is The maximum angle of the second joint under constraint is ; The range of motion of the first joint under the constraints is determined as follows: The range of motion of the second joint under constraints is determined as follows: In the formula, This represents the minimum angle of the second joint under the constraint of the third joint. This represents the maximum angle of the second joint under the constraint of the third joint. The range of motion of the third joint under constraints is determined as follows: In the formula, This represents the minimum angle of the third joint under the constraint of the second joint. This represents the maximum angle of the third joint under the constraint of the second joint. The range of motion of the fourth joint under constraints is determined as follows: Under the joint movement mode, the angles of each joint , , and If any of the components exceeds the corresponding range of motion, the palletizing robot is no longer within the workspace.

2. The method for determining the workspace of a palletizing robot according to claim 1, characterized in that, When the motion mode is Cartesian motion, the range of motion of the robotic arm end effector in the first joint coordinate system is first determined, including: The rotation angle of each joint is obtained through sensors. , , and The value is then used to calculate the position of the robotic arm end effector in the base coordinate system based on the rotation angle values ​​of each joint using the following formula. The position below is : Then calculate in the first joint coordinate system Below, the position of the robotic arm's end effector is: The end effector of the robotic arm in the first joint coordinate system The position below is denoted as Fix the first joint and calculate the six circular arcs, among which Let be the center of the circle, and its coordinates be: In the formula, The center of the first arc. The center of the second arc. The center of the third arc. The center of the fourth arc. The center of the fifth arc. The center of the sixth arc; The radii of the six arcs are as follows: In the formula, The radius of the first arc is... The radius of the second arc is... The radius of the third arc. The radius of the fourth arc. The radius of the fifth arc. Let be the radius of the sixth arc; take the area enclosed by the six arcs as the range of motion of the robotic arm end effector in the first joint coordinate system; definition and The end effector of the robotic arm is in the first joint coordinate system Coordinates of the plane: Calculate the first joint coordinate system Intersection of six circular arcs in a plane as follows: In the formula, For angle The minimum angle of the third joint under constraints. For angle The minimum angle of the third joint under constraints. For angle The minimum angle of the third joint under constraints. For angle The maximum angle of the third joint under constraints. For angle The maximum angle of the third joint under constraints. For angle The maximum angle of the third joint under constraint; definition Intersection The x-coordinate is defined. Intersection The ordinates are sorted in descending order. Arranged as , Indicate the order, sort in descending order. Arranged as , Indicates order; calculate The value, if If the value is zero, the process ends; otherwise, execution continues. Where the function The general format is ,in For a circle in the first joint coordinate system Coordinates of the center of a circle in a plane , Let be the radius of the circle, and be its coordinates. The recorded end effector of the robotic arm is in the first joint coordinate system. coordinate system The coordinates of the plane are ,if If the function value is 1, then the function value is 1; otherwise, the function value is 0. Where the function The calculation is as follows, defining the line. The equation is Define a straight line The equation is ,straight line and It formed a cone-shaped area, when and When the condition is met, the function value is 1; otherwise, the function value is 0.

3. The method for determining the workspace of a palletizing robot according to claim 2, characterized in that, The process of converting the range of motion in the first joint coordinate system to the range of motion in the base coordinate system includes: Determine the coordinate system of the robotic arm end effector on the base. lower edge shaft and The range of motion of the axis, that is, the range of motion of the end effector of the robotic arm in Cartesian motion mode. shaft and The range of motion of the shaft is determined by the following steps: The working area is defined as the range of motion of the robotic arm's end effector in the first joint coordinate system. Divide the work area into seven regions and iterate through them. If we can find that makes The first Then the end effector of the robotic arm is at the th... One region; otherwise, the robotic arm's end effector is in the seventh region; Then, based on the arc covered by the defined area, calculate in Maximum and minimum in-plane end effector motion of a robotic arm coordinate: in, and For the first The circle and the first The radius of the circle, and For the first The circle and the first The center of the circle coordinate, and For the first The circle and the first The center of the circle coordinate, for Maximum in-plane robotic arm end effector motion coordinate, for Minimum of end-effector motion of a planar robotic arm coordinate; Calculation in base coordinate system Down, Two concentric circles in a plane, and the annulus formed by the two concentric circles, determine the height. The range of motion of the lower robotic arm's end effector is also a cross-section of the workspace, with the radii of the two concentric circles being: In the formula, Let be the radius of the larger of the two concentric circles. The radius of the smaller of the two concentric circles; when and At that time, determine the end effector of the robotic arm along Range of movement: In the formula, For the end effector of the robotic arm The maximum value that can be moved. For the end effector of the robotic arm The minimum value to move; Determine the end effector of the robotic arm Range of movement: In the formula, For the end effector of the robotic arm The maximum value that can be moved. For the end effector of the robotic arm The minimum value to move; when or First, determine the straight line. and The equation: At this point, the workspace becomes a straight line. and The intersection of the conical region and the annulus determines the end effector of the robotic arm. Range of movement: Where the function The calculation is as follows, defining the line. The equation is Define a straight line The equation is ,straight line and It formed a cone-shaped area, when and When the condition is met, the function value is 1; otherwise, the function value is 0. Determine the end effector of the robotic arm Range of movement: Under the Cartesian motion pattern, the end effector of the robotic arm along The movement range and steps are as follows: Traversal Finding The first Then the end is at the th position. One region; Then, based on the arc covered by the defined region, calculate the coordinates in the first joint coordinate system. Maximum and minimum motion at the end of the plane coordinate: in, and For the first The circle and the first The radius of the circle, and For the first The circle and the first The center of the circle coordinate, and For the first The circle and the first The center of the circle coordinate, In order to be in Maximum in-plane robotic arm end effector motion coordinate, In order to be in Minimum in-plane robotic arm end effector motion Coordinates; when there is more than one solution, if finding along Maximum axis movement Then take the one that makes The minimum solution; if we are looking for the solution along... Minimum value of axis movement Then take the one that makes The minimum solution; therefore, we obtain , .

4. The method for determining the workspace of a palletizing robot according to claim 3, characterized in that, The process of generating the workspace of the palletizing robot based on the range of motion of the robotic arm's end effector in the base coordinate system includes: First joint coordinate system Get to the workspace The point corresponding to the maximum coordinate The point corresponding to the minimum value On the line segment Above, with spacing Take evenly There are points, and the coordinates of each point are . Then for each coordinate Calculate the radius at the corresponding height. and Two concentric circles, these two concentric circles are parallel to the base coordinate system. of A plane, and the annular portion enclosed by two concentric circles and a straight line. , The enclosed cone-shaped area defines a portion of the workspace: After traversing all Then, the entire workspace of the palletizing robot is obtained, in which, and The center of the first circle and the sixth circle coordinate, and The center of the first circle and the sixth circle coordinate, .

5. The method for determining the workspace of a palletizing robot according to claim 1, characterized in that, When the robotic arm's end effector grips a cargo, a diameter that can accommodate the cargo is selected as... A sphere, the cross-section of which is a circle, is uniformly sampled on the circle. Points, then in the first joint coordinate system coordinate system This is below the plane The coordinates of the points are: Traversing a collection For each point in the array, take the value that makes The number of points where the function's value is 0 is denoted as . A positive integer is preset as the threshold. , The formula for the function is: When the movement is a joint movement, if If the condition is met, the palletizing robot is no longer in the workspace, and the process ends; otherwise, the motion range of each joint under the updated constraints is as follows: in: in: In the above formula , , and The angular offset caused by the robotic arm's end effector gripping the goods is solved using the following procedure: (1) Initialize the iteration start point The end effector of the robotic arm is in the first joint coordinate system Coordinates of a plane The step size of each iteration threshold Maximum number of iterations ,definition Coordinate update formula and Coordinate update formula , get parameters ,parameter ; When the parameter to be solved is hour, , , , and Initialization is as follows: When the parameter to be solved is hour, , , , and Initialization is as follows: When the parameter to be solved is hour, , , , and Initialization is as follows: When the parameter to be solved is hour, , , , and Initialization is as follows: (2) Get the current iteration number ,parameter traverse the set For each point in the array, take the value that makes The number of points where the function's value is 0 is denoted as . , where set and The function expression is as follows: (3) If If the result is positive, proceed to step (4); otherwise, output the output. The value of the parameter to be solved is used as the value of the solution and the process ends; (4) If or If the result is positive, proceed to step (5); otherwise, output the output. The value of the parameter to be solved is used as the value of the solution and the process ends; (5) If If yes, proceed to step (6); otherwise, proceed to step (7). (6) The parameters are iteratively updated as follows: Return to step (3) after the update is complete; (7) The parameters are iteratively updated as follows: After the update is complete, return to step (3).

6. The method for determining the workspace of a palletizing robot according to claim 4, characterized in that, When the robotic arm's end effector grips a cargo, a diameter that can accommodate the cargo is selected as... A sphere, the cross-section of which is a circle, is uniformly sampled on the circle. Points, then in the first joint coordinate system coordinate system This is below the plane The coordinates of the points are: Traversing a collection For each point in the array, take the value that makes The number of points where the function's value is 0 is denoted as . A positive integer is preset as the threshold. , The formula for the function is: When the motion pattern is Cartesian motion, if If the palletizing robot is not in the workspace at this point, the process ends; otherwise, change the settings. , , and The calculation method is as follows; for others, the workspace is determined by calculating the method when the robotic arm's end effector is not gripping any goods: in, and For the first The circle and the first The radius of the circle, and For the first The circle and the first The center of the circle coordinate, and For the first The circle and the first The center of the circle coordinate, for Maximum in-plane robotic arm end effector motion coordinate, for Minimum in-plane robotic arm end effector motion coordinate, In order to be in The maximum in-plane end effector motion of a robotic arm coordinate, In order to be in Minimum in-plane robotic arm end effector motion coordinate, , , and The distance offset caused by the robotic arm's end effector gripping the goods is solved using the following procedure: (1) Initialize the iteration start point The end effector of the robotic arm is in the first joint coordinate system Coordinates of a plane The step size of each iteration threshold Maximum number of iterations ,definition Coordinate update formula and Coordinate update formula , get parameters ,parameter ; When the parameter to be solved is hour, , , , and Initialization is as follows: When the parameter to be solved is hour, , , , and Initialization is as follows: When the parameter to be solved is hour, , , , and Initialization is as follows: When the parameter to be solved is hour, , , , and Initialization is as follows: (2) Get the current iteration number ,parameter traverse the set For each point in the array, take the value that makes The number of points where the function's value is 0 is denoted as . , where set and The function expression is as follows: (3) If If the result is positive, proceed to step (4); otherwise, output the output. The value of the parameter to be solved is used as the value of the solution and the process ends; (4) If or If the result is positive, proceed to step (5); otherwise, output the output. The value of the parameter to be solved is used as the value of the solution and the process ends; (5) If If yes, proceed to step (6); otherwise, proceed to step (7). (6) The parameters are iteratively updated as follows: Return to step (3) after the update is complete; (7) The parameters are iteratively updated as follows: After the update is complete, return to step (3).

Images