A motion trajectory planning method for an ultra-redundant multi-limb segment robot

Abstract

The application provides a motion trajectory planning method of an ultra-redundant multi-limb segment robot, belongs to the nondestructive testing technical field of nuclear power plant steam generator steam-water separator cyclone vane, and combines the characteristics of the multi-limb segment robot, can quickly generate a trajectory, makes the robot perform multi-axis synchronous motion, and improves operation efficiency. According to the operation scene and the structural characteristics of the robot, the motion process is decomposed in a pre-planned path mode, the slow unfolding mode of the robot from outside to inside is inferred, the traditional kinematics equation solving is replaced by plane geometry operation, the calculation process is simplified, and the robot closely following the path motion can effectively avoid collision.

Description

Technical Field

[0001] This invention relates to the field of non-destructive testing technology for swirl blades of steam-water separators in nuclear power plant steam generators, and particularly to a motion trajectory planning method for an ultra-redundant multi-limb robotic arm. Background Technology

[0002] The planar layout of the steam generator steam-water separator blades is as follows: Figure 1 As shown, the 66 blades are closely arranged with multiple circular obstacles in the middle. The interior space is relatively low and exposed to radiation, making manual inspection risky and the inspection area limited. It is necessary to control the robot to move to a fixed position for inspection.

[0003] Unlike mobile robots, multi-limbed robotic arms need to consider not only path reachability but also obstacle avoidance during movement. Typical obstacle avoidance planning methods include artificial potential field methods, path mapping methods, fast expanding random tree methods, A* algorithms, and ant colony algorithms, most of which establish kinematic equations and solve matrix inverses using the DH parameter method. However, in jungle-like structures, using these algorithms for pathfinding requires a significant amount of time to calculate the distances between the multi-limbed robotic arm and surrounding obstacles when it is in different postures, making the process complex and lacking real-time performance. Moreover, controlling the robot's movement after finding a path is also a challenge, as the robot occupies a large area when deployed, with only the middle of the blades having sufficient space to accommodate all the limbs. Summary of the Invention

[0004] The purpose of this invention is to provide a motion trajectory planning method for a super-redundant multi-limb robotic arm, which solves the problem of motion trajectory generation for remotely controlled robots in the confined inspection environment of steam generator blades. The algorithm combines the characteristics of multi-limb robots, can generate trajectories quickly, and enables the robot to perform multi-axis synchronous motion, thereby improving work efficiency.

[0005] To achieve the above objectives, the present invention provides the following technical solution:

[0006] A motion trajectory planning method for a super-redundant multi-limb robotic arm includes the following steps:

[0007] S1: Set the endpoint M to be at the last inflection point;

[0008] S2: Calculate the number of times the endpoint M moves along its path segment (times), and set the number of moves for endpoint M. ;

[0009] S3: When At this point, the current joint point p1 is obtained from the end point M. Assume the previous joint point p0 and the current joint point p1 are on the same path segment, with the path index as... Let the current arm number be... ;

[0010] S4: If k>0, calculate p1 and the inflection point. If the distance is equal to the distance, proceed to S5 or S6; if k=0, proceed to S7.

[0011] S5: If Calculate p0, and then calculate the distance between p0 and the origin.

[0012] S6: If Calculate the minimum distance (min) and maximum distance (max) from p1 to the line segment with path index j-1;

[0013] S7: Number of moves Determine the size of moveT and times. If moveT > times, then... If moveT < times, proceed to S2; otherwise, proceed to S3.

[0014] In S1, the path index of the endpoint is: .

[0015] In S5, p0 is calculated as follows: draw a circle with p1 as the center and the arm length L[k] of the robotic arm k+1 as the radius, find the intersection point of the circle with the path segment j, and the intersection point closer to the turning point of the path is p0.

[0016] In S5, if Compare the magnitudes of Dis(p0,O) and the limit value Rmin[k-1].

[0017] like ,set up Return to S4;

[0018] like First, calculate p0, then find the difference between the angle of the first robotic arm and the initial state angle, obtain the positions of all relevant nodes, and proceed to S7.

[0019] In S5, if If the total length of the robotic arm is insufficient, the path planning is incorrect, and the motion trajectory planning ends.

[0020] In S6, if First, calculate the position of p0 on path j-1, and then perform a validity calculation on p0.

[0021] In S6, if L[k] is not within the interval, consider p0 in the previous path segment and recalculate the minimum and maximum distances from p1 to this path segment. If a p0 satisfying the condition cannot be found until j=0 (the first path segment), then consider p0 at the intersection of the two circles, refer to the appendix. Figure 5That is, the first k robotic arms are not deployed and are in the initial retracted state. Based on the difference between the angle of the first robotic arm and the angle of the initial state, the positions of all joints are deduced backward.

[0022] Compared with existing technologies, the motion trajectory planning method for a super-redundant multi-limb robotic arm provided by this invention has the following advantages:

[0023] Based on the work scenario and the structural characteristics of the robot, this invention decomposes the motion process by pre-planning the path, and deduces the way in which the robotic arm slowly unfolds from the outside to the inside. It uses planar geometric calculations instead of traditional kinematic equations to solve the problem, which simplifies the calculation process. Moreover, the robotic arm's close movement along the path can effectively avoid collisions. Attached Figure Description

[0024] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0025] Figure 1 This is a plan view of the blade layout of a steam-water separator in the prior art;

[0026] Figure 2 This is an initial state effect diagram in simulation software provided by an embodiment of the present invention;

[0027] Figure 3 This is a full trajectory effect diagram in simulation software provided by an embodiment of the present invention;

[0028] Figure 4 This is a schematic diagram of joint point calculation provided in an embodiment of the present invention;

[0029] Figure 5 This is a schematic diagram illustrating the calculation of the intersection point of two circles provided in an embodiment of the present invention;

[0030] Figure 6 This is a flowchart of the algorithm provided in an embodiment of the present invention;

[0031] Figure 7 This is a schematic diagram of the robotic arm structure provided in an embodiment of the present invention;

[0032] Figure 8 This is a diagram showing the initial state of path 1 in simulation software, as provided in an embodiment of the present invention.

[0033] Figure 9 This is a full trajectory rendering of path 1 in simulation software provided by an embodiment of the present invention;

[0034] Figure 10 This is a diagram showing the initial state of path 2 in simulation software, as provided in an embodiment of the present invention.

[0035] Figure 11 This is a full trajectory rendering of path 2 in simulation software provided in an embodiment of the present invention;

[0036] Figure 12 This is a diagram showing the initial state of path 3 in simulation software, as provided in an embodiment of the present invention.

[0037] Figure 13 This is a full trajectory rendering of path 3 in simulation software provided in an embodiment of the present invention;

[0038] Figure 14 This is a diagram illustrating the current state effect of fine-tuning in a non-initial state, provided in an embodiment of the present invention.

[0039] Figure 15 This is a diagram illustrating the effect of fine-tuning the state upon reaching the destination point in a non-initial state, as provided in an embodiment of the present invention.

[0040] Figure 16 This is a diagram showing the full trajectory effect of fine-tuning in a non-initial state, provided in an embodiment of the present invention.

[0041] Explanation of reference numerals in the attached figures:

[0042] 1. First robotic arm; 2. Second robotic arm; 3. Third robotic arm; 4. Fourth robotic arm; 5. Fifth robotic arm; 6. Sixth robotic arm; 7. Seventh robotic arm. Detailed Implementation

[0043] The following detailed description provides further details on specific implementation methods.

[0044] The robotic arm involved in this invention is such as Figure 7 As shown, it includes a first robotic arm 1, a second robotic arm 2, a third robotic arm 3, a fourth robotic arm 4, a fifth robotic arm 5, a sixth robotic arm 6, and a seventh robotic arm 7. The front end of the first robotic arm 1 is the starting point, and the end point of the seventh robotic arm 7 is the end point M. The robotic arms have joints.

[0045] Due to the limited working environment and numerous obstacles, the robot's base is fixed at the origin, and a disc-shaped inspection tool is mounted at the end. The target point is fixed during the operation, so the path of 66 blades can be planned in advance to guide the robot to move by advancing forward from the end point and gradually unfolding the robotic arm. The motion solution is obtained using plane geometry, and the joint position in each state is calculated to simulate the motion process.

[0046] In simulation software, the robot is simplified into multiple connected axis segments. After calculating the motion solution for a certain path, all trajectories are superimposed on a plane to determine whether a collision will occur. For example... Figure 2 and Figure 3 As shown, Figure 2 In the diagram, hollow circles represent blades, solid yellow circles represent obstacles, white lines represent the given path, and connected rainbow-colored segments (red, orange, yellow, green, blue, indigo, and violet represent the first robotic arm 1 to the seventh robotic arm 7, respectively) represent a multi-limbed robot. Initially, all joints rotate at 120°, derived from the rotation angle of each robotic arm relative to the previous one. At this point, the distance between the joints of each robotic arm and the origin is minimized. Figure 3 This is a superimposed image of the trajectory when the endpoint moves to the end of the path.

[0047] The robot reaches the target point from its initial state along the path, a gradual unfolding process. If the target point is close enough, the first six robotic arms (arms 1 to 6) don't need to move; only the last robotic arm (arm 7) needs to be rotated. If the path is complex and the target point is far, all robotic arms need to be unfolded. Since the path, arm length, and target point are known, we assume the robot's end effector moves equidistantly along the path, advancing a distance of moveL each time. Starting from the end of the path, we calculate backwards to the previous target point to determine the end effector position M for each motion state. From M, we calculate the position of the previous joint P1, continuing this process until all joints are found. At this point, the calculation for the current motion state ends, the end effector position M is updated, and the calculation of all joints in the new state begins.

[0048] like Figure 4 As shown, the position of the previous joint P0 is calculated using the current joint P1.

[0049] Given a path OABCD, with target point M on CD, draw a circle with M as the center and the length of the seventh robotic arm 7 as the radius. Find the intersection point of the circle and the path. Take the point closer to the turning point C of the path and denote it as P1. Draw a circle with P1 as the center and the length of arm 6 as the radius. Find the intersection point of the circle and the path and denote it as P0. Calculate the distance d between P0 and the origin O.

[0050] When finding P0, we need to first determine which line segment it will fall on. After finding the landing point, we then need to determine whether d satisfies the following two conditions:

[0051] Condition 1: ;

[0052] Condition 2: ;

[0053] Ln (n=1, 2, 3, 4, 5) represents the length of arm n, and Rmin represents the distance between the origin and the end joint of the robotic arm 5 in the contracted state.

[0054] If d satisfies both conditions, it means that the calculation of P0 is correct. Continue to find key points until all key points are found.

[0055] If d does not satisfy condition 1, it means that the robot is not long enough to reach the target point;

[0056] If d does not satisfy condition 2, it means that the distance is less than the limit value, which violates the robot's structural characteristics and is impossible. In this case, consider that P0 is not on the path segment, but on the circumference, meaning the first five robotic arms (first robotic arm 1 to fifth robotic arm 5) are not deployed. Treat them as a whole and rotate them around the origin, as follows... Figure 5 As shown, P0 lies on a circle with the origin as its center and radius Rmin, and also on a circle with P1 as its center and radius L6. Find the intersection points G and H of the two circles. Since H is closer to the path than G, the position of P0 at point H can be determined.

[0057] Given OH, P1H, and OP1, the angle θ between OP1 and OH can be calculated using the law of cosines. Since the first five robotic arms are still in a retracted state and their relative angles are fixed, we only need to calculate the difference between the angle of arm 1 and its initial angle to obtain the positions of all relevant nodes. The formula for calculating the angle difference is: .

[0058] With the horizontal direction to the right as the positive x-axis, denoted as 0 degrees, and counterclockwise rotation as the positive angle, α is the angle of the line connecting P1 and the origin relative to the positive x-axis, and β is the angle of the line connecting the end point of arm 6 and the origin relative to the positive x-axis in the initial state.

[0059] Since the path consists of several straight line segments, and the robot moves at equal intervals, some robotic arms may rotate too much when passing through the turning points of the path, resulting in excessively high instantaneous speed and acceleration, which could lead to mechanical failure. Therefore, linear interpolation is required between the two motion states to ensure a smooth transition.

[0060] like Figure 6 and 7 As shown, the present invention provides a motion trajectory planning method for a super-redundant multi-limb robotic arm, including preparation steps and calculation steps.

[0061] Preparation steps S0 are as follows:

[0062] Construct a Cartesian coordinate system with the starting point of the first robotic arm 1 as the origin, the positive x-axis as the 0-degree angle, and counterclockwise rotation as the positive angle direction.

[0063] Calculate the distance Rmin[] between each joint and the origin in the initial state. There are 7 values ​​in total. This is the minimum limit.

[0064] Set the arm length L[], and record the rotation angle angle[] of each robotic arm relative to the previous robotic arm in the current state.

[0065] Plan a path pathPoints[], the path includes the origin, the array length is n+1, there are n path segments, and record the positions of the n inflection points of the path.

[0066] Set the endpoint M to move at equal intervals along the path, with each movement being a distance moveL.

[0067] All array indices in the program start from 0.

[0068] The calculation steps are as follows:

[0069] S1: Set the endpoint M at the last inflection point, i.e. At this point, the path index of the endpoint is .

[0070] S2: Calculate how many times the endpoint M can move along its path segment using moveL, obtain the number of moves multiplied by a time factor, and set the number of moves for endpoint M. .

[0071] S3: When At this point, the current joint point p1 is obtained from the end point M. Assume the previous joint point p0 and p1 are on the same path segment, with the path index being... Let the current arm number be... .

[0072] S4: If k>0, calculate the distance between p1 and the inflection point pathPoints[j]. If k=0, it means that this state calculation is complete, and jump to step S7.

[0073] S5: If (That is, the distance between the current joint point p1 and the path inflection point pathPoints[j] is greater than or equal to the arm length of the robotic arm (k+1)), indicating that p0 and p1 can be on the same path segment. At this time, p0 can be obtained (draw a circle with p1 as the center and the arm length L[k] of the robotic arm k+1 as the radius, find the intersection of the circle with the path segment j, and the intersection point that is closer to the path inflection point is p0), and calculate the distance between p0 and the origin.

[0074] S5.1: If Then compare the magnitudes of Dis(p0,O) and the limit value Rmin[k-1].

[0075] S5.1.1: If This indicates that p0 has been found and is being set. Return to step S4.

[0076] S5.1.2: If This indicates that the first k robotic arms have not been deployed. p0 is at the intersection of the two circles. First, calculate p0, then calculate the difference between the angle of the first robotic arm 1 and the angle of the initial state. This will give you the position of all relevant nodes. Then, jump to step S7.

[0077] S5.2: If This indicates that the total length of the robotic arm is insufficient, the path planning is incorrect, and the algorithm terminates.

[0078] S6: If This indicates that p0 is on the previous or earlier path segment. Calculate the minimum and maximum distances min and max from p1 to the line segment with path index j-1.

[0079] S6.1: If Then, we can first calculate the position of p0 on path j-1, and then perform a validity calculation on p0. First, obtain the preliminary position of p0 according to S5, and then judge its validity through S5.1 and S5.2. The specific operation is the same as steps S5.1 and S5.2.

[0080] S6.2: If L[k] is not in the interval (i.e. L[k] is not in the min~max interval), then consider that p0 is on a previous path segment, j=j-1, and keep searching until j=0. If p0 cannot be found, then consider that the end of arm k+1 is not on the path, but on the circumference, and use this to deduce the position of all joints.

[0081] S7: Number of moves Determine the size of moveT compared to times.

[0082] S7.1: If This indicates that the current path segment has been completed. If the distance is found to be within the specified range, the algorithm ends. Otherwise, the remaining distance of the current path segment needs to be calculated, the distance is supplemented in the previous path segment, the end point M is reassigned, and the algorithm returns to step S2.

[0083] S7.2: If Return to step S3.

[0084] When the robot is in its initial state, it moves along the inspection path of 66 blades, completely avoiding obstacles, such as... Figures 8 to 13 The trajectory diagram is shown in the initial state.

[0085] When the robot is not in its initial state, by inputting the current state and subsequent path, and following the calculation method described above, the next motion trajectory can also be calculated, enabling online fine-tuning. Figure 14-16 The diagram shows a fine-tuning process in a non-initial state.

[0086] This invention utilizes a multi-limbed robot with a fixed base and an end-effector tool to inspect 66 blades within the complex working space of a nuclear power plant steam generator. The robot arm is required to follow a given path to reach the center of the blade and calculate all motion states from the initial posture to the end-effector reaching the target point to obtain its motion trajectory.

[0087] This invention avoids complex calculations and formulates a method for rapid movement, and obtains the changes of each joint in a timely manner during the movement. Combined with the fact that the center of each blade is known in the actual application scenario, this invention adopts a pre-planned path method to make the robot move as close to the path as possible, and achieves the purpose by having the end effector drive the front joint to slowly unfold.

[0088] The above description is merely a specific embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any variations or substitutions that can be easily conceived by those skilled in the art within the technical scope disclosed in the present invention should be included within the scope of protection of the present invention. Therefore, the scope of protection of the present invention should be determined by the scope of the claims.

Claims

1. A motion trajectory planning method for a super-redundant multi-limb robotic arm, characterized in that, Includes the following steps: S1: Set the endpoint M to be located at the last inflection point. The path index of the endpoint is... n is the number of path segments; S2: Calculate the number of times the endpoint M moves along its path segment (times), and set the number of moves for endpoint M. ; S3: When At this point, the current joint point p1 is obtained from the end point M. Assume the previous joint point p0 and the current joint point p1 are on the same path segment, with the path index as... Let the current arm number be... ; S4: If k>0, calculate p1 and the inflection point. If the distance is equal to the distance, proceed to S5 or S6; if k=0, proceed to S7. S5: If Calculate p0, then calculate the distance between p0 and the origin. The method for calculating p0 is as follows: draw a circle with p1 as the center and the arm length L[k] of the robotic arm k+1 as the radius, find the intersection point of the circle and the path segment j, and the intersection point closer to the turning point of the path is p0; if Compare Dis(p0,O) with the limit value Rmin[k-1]. ,set up Return to S4; if First calculate p0, then find the difference between the angle of the first robotic arm and the initial state angle to obtain the positions of all relevant nodes, and proceed to S7; if If the total length of the robotic arm is insufficient, the path planning is incorrect, and the motion trajectory planning ends. S6: If Calculate the minimum distance (min) and maximum distance (max) from p1 to the line segment with path index j-1; if First, calculate the position of p0 on path j-1, and then perform a reasonableness calculation on p0. If L[k] is not in the interval, consider p0 on the previous path segment and recalculate the distance min and max from p1 to this path segment. If no p0 that meets the conditions can be found until j=0, consider p0 at the intersection of the two circles. The first k robotic arms are not deployed and are in the initial contracted state. Based on the difference between the angle of the first robotic arm and the angle of the initial state, deduce the position of all joints backward. S7: Number of moves Determine the size of moveT and times. If moveT > times, then... If moveT < times, proceed to S2; otherwise, proceed to S3.

Images