Method and device for motion planning of a four-legged robot for steam generator tube plate

Abstract

The application discloses a motion planning method and equipment for a steam generator tube plate quadruped robot, and belongs to the technical field of motion planning of a nuclear industry steam generator quadruped robot. In order to solve the problems that the existing robot has low planning efficiency when inspecting a steam generator tube plate, and the execution success rate of a planning scheme needs to be improved, the application firstly performs traversal search to determine the motion space of a base and a foot end, and determines a feasible foot landing set; then divides a given polyline path into multiple straight line paths, plans a base track, determines the base position, and for each straight line path, the robot selects a foot landing point in the feasible foot landing set according to a turning motion evaluation index, continuously performs a turning motion process until the robot adjusts the posture to face a given orientation and completes the turning motion; the robot selects a foot landing point in the feasible foot landing set according to a straight line motion evaluation index, continuously repeats a straight line motion process until the base of the robot reaches the terminal point of the straight line path.

Description

Technical Field

[0001] This invention belongs to the field of motion planning technology for quadruped robots in nuclear industry steam generators, specifically relating to a motion planning method, storage medium, and device for a quadruped robot for steam generator tube sheets. Background Technology

[0002] Nuclear energy now accounts for a significant proportion of modern energy. In the world's operational nuclear power plants, over 70% of reactor units utilize pressurized water reactors. The heat generated by the pressure vessel is transferred to the steam generator via the primary loop, then to the secondary loop, ultimately driving the generator set to produce electricity. Heat transfer tubes, fixed to the evaporator tube sheet, are critical components for heat transfer within the evaporator and require regular inspection and maintenance. Due to the high radioactivity of nuclear power plants, using robots equipped with eddy current tools to inspect the heat transfer tubes inside the steam generator is currently the mainstream approach. Because the specifications of the heat transfer tubes and their distribution on the tube sheet vary, various models of maintenance robots are available, such as ZR-100, PEGASYS, and HIT-Crawler. These robots, equipped with eddy current tools, can only inspect heat transfer tubes of a specific tube sheet distribution, leading to complex and inefficient motion planning processes. Furthermore, the robots are prone to execution failures when implementing planned motion schemes, and the success rate of planned scheme execution needs improvement. To improve adaptability to different types of evaporator tube sheets, a quadruped tube sheet inspection robot is needed, which can carry inspection tools and move flexibly on evaporator heat transfer tube sheets of various specifications.

[0003] Because the heat transfer tubes of the steam generator are numerous, the process of manually operating the maintenance robot is too complicated and prone to errors. Therefore, it is necessary to develop a motion planner to transform the walking task into the required planned motion, so as to ensure that the quadruped robot moves efficiently and safely on the tube sheet. Summary of the Invention

[0004] This invention addresses the problems of low planning efficiency and the need to improve the success rate of planning scheme execution when using existing robots to inspect steam generator tube sheets.

[0005] A motion planning method for a quadruped robot for a steam generator tube sheet, the quadruped robot comprising a base module, a working arm module, four leg modules, and four toe modules;

[0006] Four leg modules are mounted on the base module. A passive rotary joint is located at the center of the base module. Each leg module includes a foot and a rotary joint connecting the foot to the base. The rotary joint connecting the foot to the base is designated as the first rotary joint. The foot has two rotary joints. The rotary joint closer to the first rotary joint is designated as the second rotary joint, and the other rotary joint is designated as the third rotary joint. These three rotary joints determine the robot's movement in the plane. The combined relative motion of the base module and the four leg modules is used to enable the robot to walk on the tube sheet.

[0007] Each foot end is equipped with a movable joint, and the four toe modules are respectively set at the ends of the four leg modules through movable joints. The toe modules can be lifted and gripped to fix with the tube plate, and can be released and lowered to detach from the tube plate.

[0008] The motion planning method includes the following steps:

[0009] First, a traversal search is performed to determine the motion space of the base and the foot, and to determine the set of feasible footing points: for a pipe hole, if the robot's forward and inverse kinematic conditions are met, the footing position does not block the pipe, and the foot does not collide with the evaporator water chamber after landing, then this point is a feasible footing point; by traversing the foot working space, the set of all feasible footing points at this position can be obtained.

[0010] The given polyline path is then divided into multiple straight paths. Based on each straight path, the base trajectory is planned, and the base position is determined. For each straight path segment, there is a starting point, an ending point, and a direction of movement. The robot selects a landing point from the feasible landing set based on the turning motion evaluation index and continuously performs the turning motion until the robot adjusts its posture to face the given orientation, completing the turning motion. The robot selects a landing point from the feasible landing set based on the straight motion evaluation index and continuously repeats the straight motion process until the robot base reaches the end of the straight path. This achieves the robot's movement on the given polyline path.

[0011] The process by which the robot selects a landing point from the set of feasible landing points based on the turning motion evaluation index includes the following steps:

[0012] A1. Define the evaluation index of the robot's base steering motion based on the robot's initial motion posture:

[0013] 1) Minimal Indicator 1: The angle between the above-mentioned orientation and the given direction;

[0014] 2) Miniature indicator 2: The difference between the angle of the foot relative to the base and the standard angle of 45°;

[0015] 3) Extremely large index 3: The size of the set of feasible landing points after landing;

[0016] A2. Based on the steering motion evaluation index, plan the robot's base steering motion: Use the TOPSIS method to perform positive and standardization processing on the three steering motion evaluation indexes respectively, and obtain the optimal and worst solutions for the three indexes. According to the weight of the three steering motion evaluation indexes, calculate the steering motion score of each landing point, and select the landing point with the highest score to land and realize the steering motion.

[0017] The process by which the robot selects a landing point from the set of feasible landing points based on linear motion evaluation metrics includes the following steps:

[0018] B1. Analyze the linear motion form of the robot and define the evaluation index of the robot's linear motion:

[0019] a) Minimal indicator a: The distance the foot point moves along a given path;

[0020] b) Minimal indicator b: The difference between the angle of the foot relative to the base and the standard angle of 45°;

[0021] c) Minimalism index c: The distance between the robot's center of gravity and the direction of movement;

[0022] d) Extremely large index d: The size of the set of feasible landing points after landing;

[0023] B2. Based on the linear motion evaluation indicators, plan the robot's linear motion:

[0024] Using the TOPSIS method, the four evaluation indicators of linear motion are positiveized and standardized respectively, and the optimal and worst solutions of the four indicators are obtained. Based on the weights of the four linear motion evaluation indicators, the linear motion score of each landing point is calculated, and the landing point with the highest score is selected to achieve linear motion.

[0025] Furthermore, in the process of assembling all feasible landing points, the robot's forward kinematics conditions are satisfied, namely, the following forward kinematics solution:

[0026] For a square tube sheet, the robot base position is B(r). B ,c B ), where r B c B The row and column coordinates of the heat transfer tubes on the square tube sheet corresponding to the robot base are given relative to the plane of the tube sheet; the positions of each foot (r) are obtained. i c i ):

[0027]

[0028] Where, α i=45+90·(i-1) represents the angle of rotation of the pivot connecting each foot i to the fuselage relative to the fixed tube plate, i=1,2,3,4; a1 is the distance between the pivot axis of the first rotary joint and the pivot axis of the second rotary joint, a2 is the distance between the pivot axis of the third rotary joint and the axis of the movable joint, which is actually the length of the two foot arms at the foot end of each leg module; 2b and 2w represent the distance between the pivot axes of two adjacent first rotary joints.

[0029] Furthermore, in the process of setting up all feasible landing points, the inverse kinematics conditions of the robot are satisfied, namely, the following inverse kinematics solution:

[0030] The positions of the three supporting feet relative to the base are F1(r1,c1), F2(r2,c2), and F3(r3,c3), respectively. The rotation angles θ of the two rotational joints of foot j are obtained. 2j and θ 3j ;

[0031]

[0032] If there are solutions for the two turns of foot j, then this is a feasible inverse kinematics solution / solution pair.

[0033] Furthermore, during the process of traversing and searching to determine the motion space of the base and the feet, a method for regularizing the robot's motion space is adopted. The method for regularizing the robot's motion space is as follows:

[0034] Based on the position and orientation of the base, and according to the settings and movement range of the four leg modules of the quadruped robot, the position and orientation range of each foot is determined as a truncated annular region; at the same time, based on the position of the four feet of the robot, the actual movement space of the base is determined as the region where the annular regions corresponding to the position and orientation ranges of the four feet or the annular regions corresponding to the position and orientation ranges of the three feet intersect.

[0035] The motion space is processed using the envelope method. Considering the distribution of tube holes in the tube sheet, the minimum envelope rectangle of the base is used to replace the base motion space, and the minimum envelope rectangle of the foot end is used to replace the foot motion space. The minimum envelope rectangle is the rectangle with the smallest area that can completely contain the foot or base motion space.

[0036] Furthermore, the process of planning the base trajectory and determining the base location based on each straight path includes the following steps:

[0037] 3.1 Solving for the optimal ideal base position using the Lagrange multiplier method:

[0038] Let the starting position of the base be P. s (x s ,y sThe final position P in the direction of motion e (x e ,y e Given that the coordinates of the two corner points of the rectangular bounding space of the base under the current supporting foot are R1(a,c) and R2(b,d), then the optimal ideal base position P is... b The optimization problem for (x,y) is described as follows:

[0039]

[0040] subject to

[0041] g1(x,y)=-x+min(a,b)≤0

[0042] g2(x,y)=x-max(a,b)≤0

[0043] g3(x,y)=-y+min(c,d)≤0

[0044] g4(x,y)=y-max(c,d)≤0

[0045] Where, |||2 represents the L2 norm, such as

[0046] g1(x,y)-g4(x,y) ensures that the x-coordinate of the optimal ideal base position is within the rectangular range; these four constraints and the optimization function ensure that the endpoint of the base trajectory is closest to the direction of motion, and the optimal solution is that the endpoint is on a straight line in the direction of motion.

[0047] Introducing the Lagrange multipliers μ{μ1,μ2,μ3,μ4}, we obtain the corresponding Lagrange function as follows:

[0048]

[0049] The solution set for the ideal base position is obtained by introducing the KKT conditions. Using the distance from the endpoint of the linear motion as the evaluation index, the optimal ideal base position P can be obtained. b (x * ,y * );

[0050] 3.2 Solving for the actual base trajectory:

[0051] In P b (x * ,y * Near the point of motion, with the requirement of satisfying the feasible solution of inverse kinematics, a traversal search is performed to obtain the feasible base positions along the motion direction under each attitude.

[0052] Furthermore, the KKT conditions are introduced to obtain the solution set for the ideal base position. The KKT conditions mentioned in the process are as follows:

[0053]

[0054] in, Let L(x,y,λ,μ) denote the partial derivatives of L(x,y,λ,μ) in the x and y directions.

[0055] Furthermore, the specific process of planning the robot's base steering motion based on the steering motion evaluation index, as described in A2, includes the following steps:

[0056] The left front leg of the robot body is defined as the lf leg, the left rear leg as the lb leg, the right front leg as the rf leg, and the right rear leg as the rb leg. During movement, each leg lands in the gait sequence of lf-rf-rb-lb. Before each landing, the base needs to be adjusted with the support of the other three legs. Turning movement only considers rotating the base, and the goal is to rotate to the target angle within the movement space of the base.

[0057] The steps for calculating the steering motion score are as follows:

[0058] A201, Data Forwarding:

[0059] The p-th feasible landing point is evaluated, where q represents the index of the maximum / minimum index, and x... pq This refers to the raw data, i.e., the actual values ​​of each indicator. This is the data obtained after forward processing;

[0060] Positiveing ​​of extremely small indicators:

[0061]

[0062] Positive transformation of extremely large indicators:

[0063]

[0064] A202. Standardize all positively evaluated indicators, and denote the standardized data as z. pq :

[0065]

[0066] A203. Find the optimal and worst solutions:

[0067] Optimal solution vector:

[0068] z best =[z best1 ,z best2 ,z best3 ] = [maxz p1 ,maxzp2 ,maxz p3 ]

[0069] worst solution vector:

[0070] z worst =[z worst1 ,z worst2 ,z worst3 ] = [minz p1 ,minz p2 ,minz p3 ]

[0071] Where p is the calculated landing point number, p = 1, 2, ..., n, and n is the number of all feasible landing points;

[0072] A204. Calculate the turning motion score S at each landing point. p :

[0073]

[0074] Among them, w q The weights of each indicator;

[0075] A205. Based on the score ranking, select the highest-scoring footing point to land and achieve the turning motion.

[0076] Furthermore, the highest score landing point is...

[0077] A computer storage medium storing at least one instruction, which is loaded and executed by a processor to implement the motion planning method for a steam generator tube sheet quadruped robot.

[0078] A motion planning device for a quadruped robot on a steam generator tube sheet is provided. The device includes a processor and a memory. The memory stores at least one instruction, which is loaded and executed by the processor to implement the motion planning method for the quadruped robot on a steam generator tube sheet.

[0079] The beneficial effects of this invention are:

[0080] This invention can effectively plan the feasible movement space of a quadruped robot for steam generator tube sheets, and plan the ideal base position and landing point of the robot based on the feasible movement space, ensuring the success rate of the planning scheme; at the same time, the solution of this invention greatly improves the planning efficiency when the robot is repairing the steam generator tube sheet.

[0081] Given five sets of straight paths with obstructed pipe holes, the planning effect using the method proposed in this invention is as follows:

[0082] E1 600 100％ 19 E2 600 100％ 25 E3 606 100％ 22 E4 432 100％ 44 E5 739 100％ 47 Attached Figure Description

[0083] Figure 1 The process of motion planning for a tube sheet quadruped robot.

[0084] Figure 2 Let be the coordinate system of the tube sheet quadruped robot.

[0085] Figure 3 This is a schematic diagram of a tube sheet quadruped robot.

[0086] Figure 4 This is a schematic diagram of the foot movement of a tube sheet quadruped robot using a 2R-type serial robotic arm.

[0087] Figure 5 This is a schematic diagram of the base movement of a tube sheet quadruped robot moving in a 3DOF manner with three legs supporting it.

[0088] Figure 6 This is a schematic diagram of the base movement when the tube sheet quadruped robot moves in a 3DOF manner and is supported by four legs.

[0089] Figure 7 This is a two-dimensional schematic diagram of a square tube sheet and a quadruped robot with a tube sheet.

[0090] Figure 8 The motion space of a tube-plate quadruped robot is solved using the Monte Carlo method.

[0091] Figure 9 The rectangle represents the motion space envelope of the tube sheet quadruped robot.

[0092] Figure 10 This is a two-dimensional illustration of five sets of linear tasks.

[0093] Figure 11 The method for defining the angles of each leg of the tube sheet quadruped robot.

[0094] Figure 12 This is the robot turning process in the second set of experiments.

[0095] Figure 13 This is the linear motion process of the robot in the second group of experiments. Detailed Implementation

[0096] It should be noted that, where there is no conflict, the various embodiments disclosed in this application can be combined with each other.

[0097] Specific implementation method one: Refer to Figures 1 to 3 This implementation method will be described in detail.

[0098] This embodiment is a motion planning method for a quadruped robot on a steam generator tube sheet. Given a motion path on the tube sheet, the method plans the specific motion of the quadruped robot along the given path, ensuring the robot's motion efficiency and stability, provided that the mechanical structure motion limits of the quadruped robot are not exceeded, the robot does not move to the blind holes of the evaporator tube sheet, and the robot does not collide with the water chamber boundary.

[0099] For the motion planning of the tube sheet quadruped robot, the input conditions are the initial robot position and orientation, and the given polyline motion path. The solution process first calculates the motion space of the robot's feet and base according to the forward and inverse kinematics formulas. The given polyline path is decomposed into multiple straight path segments. For each straight path segment, the motion planning of this straight path segment is realized by planning turning motion and planning straight motion, thus obtaining the motion planning of the complete polyline path.

[0100] The motion planning method for a quadruped robot on a steam generator tube sheet described in this embodiment includes the following steps:

[0101] Step 1: Based on the structure of the tube sheet quadruped robot, solve for the simplified formulas of forward and inverse kinematics:

[0102] 1.1 Analysis of the robot's motion patterns:

[0103] The structure of a quadruped robot is as follows Figure 3 As shown, it includes a base module 10, an operating arm module 30, four leg modules 20, and four toe modules 40;

[0104] Four leg modules are mounted on the base module; a passive rotary joint is located at the center of the base module, such as... Figure 3 In the rotation marker 1, each leg module includes a foot end and a rotational joint connecting the foot end to the base. The rotational joint connecting the foot end to the base is designated as the first rotational joint. The foot end has two rotational joints; the one closer to the first rotational joint is designated as the second rotational joint, and the other is designated as the third rotational joint. Figure 3 Rotation markers 2 and 3 in the diagram indicate that these three types of rotary joints determine the robot's movement in the plane; the combined relative motion of the base module and the four leg modules is used to enable the robot to walk on the tube sheet;

[0105] The working arm module is installed below the base module and carries eddy current tools to inspect the heat transfer tubes;

[0106] Each foot tip is equipped with a movable joint, such as Figure 3 The four toe modules are located at the ends of the four leg modules via movable joints. The toe modules can be lifted and gripped to fix themselves to the tube plate, and can be released and lowered to detach from the tube plate.

[0107] The quadruped robot in this embodiment is a legged robot for overhauling heat transfer tubes of multi-specification steam generators, as described in application number 202210079518.3.

[0108] The movement of a quadruped robot includes two types of movement: movement of the feet and movement of the robot's body, and the two movements can be performed simultaneously.

[0109] like Figure 2 As shown, a DH coordinate system is established based on the robot's configuration. The DH parameters of the tube sheet quadruped robot are shown in Table 1.

[0110] Table 1

[0111]

[0112] Link 4 in Table 1 is actually a sliding joint.

[0113] like Figure 4 As shown, the movement of the foot involves three legs supporting the foot, while the remaining leg moves. At this time, the moving leg, together with the robot base, is considered as a planar 2R robot to achieve two degrees of freedom in planar motion.

[0114] The movement of the quadruped robot's base is not directly generated, but rather produced by the contact force between the feet and the tube sheet. For example... Figure 5 and Figure 6 As shown, when the base moves in a three- or four-legged supporting configuration, the tube sheet is considered as part of the robot, and the tube sheet is as follows: Figure 7 As shown, the tube sheet, together with the feet, legs and robot base fixed on it, form a parallel robot. The movement of the feet drives the movement and rotation of the base, realizing the movement of the base module in the plane with three degrees of freedom (2 translational degrees of freedom and 1 rotational degree of freedom).

[0115] 1.2. Forward kinematics solution:

[0116] For a square tube sheet, the robot base position is B(r). B ,c B ), where r B c B Let the row and column coordinates of the heat transfer tubes on the square tube sheet corresponding to the robot base be represented by their positions relative to the tube sheet plane. By simplifying the forward kinematics calculation formula, the positions (r) of each foot can be obtained. i c i ):

[0117]

[0118] Where, α i=45+90·(i-1) represents the angle of rotation of the pivot connecting each foot i to the fuselage relative to the fixed tube plate, i=1,2,3,4; a1 is the distance between the pivot axis of the first rotary joint and the pivot axis of the second rotary joint, a2 is the distance between the pivot axis of the third rotary joint and the axis of the movable joint, which is actually the length of the two foot arms at the foot end of each leg module; 2b and 2w represent the distance between the pivot axes of two adjacent first rotary joints.

[0119] 1.3 Inverse kinematics solution:

[0120] The positions of the three supporting feet relative to the base are F1(r1,c1), F2(r2,c2), and F3(r3,c3), respectively. The simplified inverse kinematics calculation formula yields the rotation angles θ of the two rotational joints of foot j. 2j and θ 3j If there are solutions for the two rotation angles of foot j, then this is a feasible inverse kinematics solution / solution pair (substituting the base position and the obtained two rotation angles into the above forward kinematics solution formula, the obtained foot j position is the same as the actual position, which can also verify the correctness of the solution).

[0121]

[0122] Step 2: Traverse and search to determine the motion space between the base and the foot:

[0123] If we use the Monte Carlo method to solve for the motion space range according to the formula in step 1, we can see that the motion space is not a regular region, which makes the search calculation very difficult. Therefore, this invention proposes a method for regularizing the robot's motion space based on the idea of ​​the envelope method.

[0124] Given the position and orientation of the base, the actual movement space of the foot is as follows: Figure 8 As shown in (a), this range actually lies within a circular region truncated by two straight lines, i.e. Figure 8 The shaded area of ​​the truncated ring in (b) is shown; given the positions of the four feet, the actual motion space of the base is as follows. Figure 8 As shown in (c), the actual range lies within the area formed by the intersection of four or three circular rings, as... Figure 8 (b) The shaded area at the center is because for a fixed foot position, the fixed foot position is equivalent to a "fixed base", while the actual base is equivalent to a "moving foot". The fixed foot achieves the movement of the actual base through the rotation of two rotating joints. Therefore, the movement space of the actual base is within a circle (the radius of the circle is the sum of the rod length from the fixed foot moving joint axis to the rotation axis 2 and the rod length from the rotation axis 2 to the rotation axis 1).

[0125] The motion space is processed using the envelope method. Considering the distribution of tube holes in the tube sheet, the minimum envelope rectangle of the base is used to represent the base motion space, and the minimum envelope rectangle of the foot is used to represent the foot motion space. The minimum envelope rectangle is the rectangle with the smallest area that can completely contain the foot or base motion space, which facilitates the subsequent planning and search. Therefore, the envelope rectangles of the foot motion space and the base motion space are first determined. For the foot motion space, the enveloping motion region must be a truncated ring. Based on the four endpoints of the truncated ring and the tangents of the four circles (the base relative to the four foot motion spaces), the envelope rectangle of the foot motion space can be obtained, such as... Figure 9 The truncated rings in (a) and (b) are shown; for the base motion space, the motion region of the envelope must be the intersection of three or four circles. Based on these intersections and the tangents of each circle, the envelope rectangle of the base motion space can be obtained.

[0126] Step 3: Based on the motion space of the base, and according to the given motion path, plan the trajectory of the base, that is, determine the position of the base:

[0127] 3.1 Solving for the optimal ideal base position using the Lagrange multiplier method:

[0128] Let the starting position of the base be P. s (x s ,y s The final position P in the direction of motion e (x e ,y e Given that the coordinates of the two corner points of the rectangular bounding space of the base under the current supporting foot are R1(a,c) and R2(b,d), then the optimal ideal base position P is... b The optimization problem for (x,y) can be described as follows:

[0129]

[0130] subject to

[0131] g1(x,y)=-x+min(a,b)≤0

[0132] g2(x,y)=x-max(a,b)≤0

[0133] g3(x,y)=-y+min(c,d)≤0

[0134] g4(x,y)=y-max(c,d)≤0

[0135] Where, |||2 represents the L2 norm, such as

[0136] The constraints g1(x,y)-g4(x,y) ensure that the x-coordinate of the optimal ideal base position lies within the rectangular area. These four constraints and the optimization function guarantee that the endpoint of the base trajectory is closest to the direction of motion, and the optimal solution is that the endpoint lies on a straight line in the direction of motion.

[0137] Introducing the Lagrange multipliers μ{μ1,μ2,μ3,μ4} (there are no equality constraints here, so there is no need to introduce the multiplier λ), we obtain the corresponding Lagrange function as follows:

[0138]

[0139] Introducing KKT conditions:

[0140]

[0141] The solution set of the ideal base position can then be obtained. Using the distance from the endpoint of the linear motion as the evaluation index, the optimal ideal base position P can be obtained. b (x * ,y * ).

[0142] 3.2 Solving for the actual base trajectory:

[0143] In P b (x * ,y * Near the point of origin, satisfying the feasible solution of inverse kinematics is a necessary condition. By traversing the search, the feasible base positions along the direction of motion in each attitude can be obtained. By continuously using this method to search during the motion, the actual base motion trajectory can be obtained.

[0144] Step 4: Based on the robot's base position and according to motion evaluation metrics, plan the robot's motion for the given path:

[0145] 4.1 Determine the set of feasible landing sites for the robot:

[0146] For a given pipe opening, a feasible landing point is one that satisfies the robot's forward and inverse kinematic conditions, ensures the landing position is not blocked by the pipe, and prevents collision with the evaporator water chamber after landing. Based on the robot's base position, traversing the rectangle of the footwork space yields the set of all feasible landing points at that location.

[0147] 4.2 Analyze the robot's initial motion posture and define the evaluation index for the robot's base steering motion:

[0148] Analysis of the robot's motion and working environment reveals that a good initial posture for the robot should meet the following conditions:

[0149] Of the four orientations of the fuselage, one orientation is as consistent as possible with the given direction; along this orientation, the four legs are evenly distributed and "spread out" to facilitate subsequent movement; this posture ensures that the swinging legs have sufficient feasible landing space when starting to move, increasing the possibility of movement and avoiding the situation of "being unable to move" when encountering dynamic obstacles during movement.

[0150] These conditions translate into corresponding evaluation indicators for the base steering motion:

[0151] 1) Minimal Indicator 1: The angle between the above orientation and the given direction.

[0152] 2) Miniature indicator 2: The difference between the angle of the foot relative to the base and the standard angle of 45°.

[0153] 3) Extremely large index 3: The size of the set of feasible landing points after landing.

[0154] 4.3. Based on the steering motion evaluation indicators, plan the robot's base steering motion:

[0155] The TOPSIS method is used to determine the optimal solution that meets the objective from a finite number of alternatives (i.e., all feasible solutions in the motion space) by minimizing the distance to the ideal point and maximizing the distance to the worst point. Based on the robot's motion space, the turning motion score of each footing point in each posture is evaluated, and the footing point with the highest score is selected each time to complete the base turning motion.

[0156] The robot's left front leg is defined as the lf leg, the left rear leg as the lb leg, the right front leg as the rf leg, and the right rear leg as the rb leg. During movement, each leg lands in a gait sequence of lf-rf-rb-lb. Before each landing, the base needs to be adjusted with the other three legs supporting it. Turning movements only consider rotating the base, aiming to reach the target angle within the base's movement space.

[0157] The steps for calculating the steering motion score are as follows:

[0158] 1) Data forwarding:

[0159] The p-th feasible landing point is evaluated (there are n feasible landing points in total), where q represents the index of the maximum / minimum index, and x... pq This refers to the raw data, i.e., the actual values ​​of each indicator. This is the data obtained after forward processing.

[0160] Positiveing ​​of extremely small indicators:

[0161]

[0162] Positive transformation of extremely large indicators:

[0163]

[0164] 2) Data standardization:

[0165] Standardize all positive metrics; the standardized data is as follows:

[0166]

[0167] 3) Find the optimal and worst solutions:

[0168] Optimal solution vector:

[0169] z best =[z best1 ,z best2 ,z best3 ] = [maxz p1 ,maxz p2 ,maxz p3 ]

[0170] worst solution vector:

[0171] z worst =[z worst1 ,z worst2 ,z worst3 ] = [minz p1 ,minz p2 ,minz p3 ]

[0172] Where p is the calculated landing point number, p = 1, 2, ..., n, and n is the number of all feasible landing points;

[0173] 4) Calculate the turning motion score S at each landing point. p :

[0174]

[0175] Among them, w q The weights of each indicator.

[0176] 5) Score and sort, then select the highest score landing point i. * To land and achieve turning motion

[0177]

[0178] 4.4 Analyze the linear motion patterns of the robot and define the evaluation index for the robot's linear motion:

[0179] The robot's movement must meet the following conditions:

[0180] The robot should move as far as possible along a given direction to ensure its speed; after landing, the robot's orientation should not deviate too much from the given direction; the distance between the robot's center of gravity and the direction of movement should be as small as possible to ensure stability during movement; and there should be sufficient space for the next leg to land after landing.

[0181] These conditions translate into corresponding evaluation indicators for the linear motion of the base:

[0182] a) Minimal indicator a: The distance the foot point moves along a given path.

[0183] b) Minimal indicator b: The difference between the angle of the foot relative to the base and the standard angle of 45°.

[0184] c) Minimalism indicator c: The distance between the robot's center of gravity and the direction of movement.

[0185] d) Extremely large index d: The size of the set of feasible landing points after landing.

[0186] 4.5. Based on the linear motion evaluation index, plan the robot's linear motion:

[0187] Similar to the method in step 4.3, the TOPSIS method is also used to positively process and standardize the four evaluation indicators of linear motion, find the optimal and worst solutions of the four indicators, calculate the linear motion score of each landing point according to the weight of the four linear motion evaluation indicators, and select the landing point with the highest score to achieve linear motion.

[0188] Step 5: Decompose the given polyline path into multiple linear motions, and plan the motion of the tube sheet quadruped robot based on the results of Step 4:

[0189] The given polyline path is divided into multiple straight paths. For each straight path, there is a starting point, an ending point, and a direction of movement. Based on the turning motion evaluation index from step 4.2, the robot selects a landing point using the method in step 4.3, repeating this process until the robot adjusts its posture to face the given direction, completing the turning motion. Next, based on the straight motion evaluation index from step 4.4, the robot selects a landing point using the method in step 4.5, repeating this process until the robot base reaches the ending point of the straight path. Each straight path uses the above method to realize the robot's movement on the given polyline path.

[0190] Example

[0191] Given a straight line, task 2 of the five experiments, such as... Figure 10As shown, the robot's pose is represented by an 11-element tuple, where each element represents the base row coordinates, base column coordinates, base rotation angle, lf-axis 1 rotation angle, lf-axis 2 rotation angle, rf-axis 1 rotation angle, rf-axis 2 rotation angle, lb-axis 1 rotation angle, lb-axis 2 rotation angle, rb-axis 1 rotation angle, and rb-axis 2 rotation angle. The robot's initial pose for this task is [15,23,17,29.57,-75.61,-29.23,76.43,-24.17,81.44,31.66,-89.89], the initial position of the path is [15.0,23.0], the ending position is [24.0,23.0], and the path angle is 90°. The angle parameters in the robot pose are defined as follows: Figure 11 As shown, the two rotation axes are initialized to 0°, the angle increases in the clockwise direction and decreases in the counterclockwise direction.

[0192] First, the turning motion is planned. The weights of the three indicators are 0.2, 0.3, and 0.5, respectively. The robot first lands on its left leg. According to forward kinematics calculations, the set of possible landing points is [[21,31],[21,32],[22,31],[22,32],[23,29],[23,30],[23,31],[23,32],[24,28],[24,30],[25,26],[25,27],[25,29],[25,30],[26,28],[26,29],[27,26],[24,29]]. Figure 12 As shown in the first image, after evaluation using the Topsis method, the highest-scoring foot placement point, [23, 31], is selected to initiate the RF leg movement. The set of possible foot placement points is as follows: Figure 12 As shown in the second image, this process is repeated until the base is rotated to the specified angle and the legs are evenly distributed.

[0193] Then, linear motion planning is performed. The weights of the four indicators are 0.4, 0.1, 0.3, and 0.2, respectively. The robot first lands on its left (LF) leg. Based on forward kinematics calculations, the score for each landing point is obtained. The landing point with the highest score is selected for the LF leg landing. Then, the landing of the right (RF) leg is planned, and this process is repeated until the robot base reaches the end of the path. The robot's motion process is as follows: Figure 13 As shown, this only shows the robot's state after all four legs have landed.

[0194] Given five sets of straight paths with obstructed orifices, the method proposed in this invention is used for planning.

[0195] E1 600 100％ 19 E2 600 100％ 25 E3 606 100％ 22 E4 432 100％ 44 E5 739 100％ 47 Specific Implementation Method Two:

[0197] This embodiment is a computer storage medium that stores at least one instruction. The at least one instruction is loaded and executed by a processor to implement the motion planning method for a steam generator tube sheet quadruped robot.

[0198] It should be understood that the instructions include computer program products, software, or computerized methods corresponding to any method described in this invention; the instructions can be used to program computer systems or other electronic devices. Computer storage media may include readable media on which instructions are stored, and may include, but are not limited to, magnetic storage media, optical storage media; magneto-optical storage media include read-only memory (ROM), random access memory (RAM), erasable programmable memory (e.g., EPROM and EEPROM), and flash memory layers, or other types of media suitable for storing electronic instructions. Specific implementation method three:

[0200] This embodiment is a motion planning device for a quadruped robot on a steam generator tube sheet. The device includes a processor and a memory. It should be understood that this includes any device described in this invention that includes a processor and a memory. The device may also include other units or modules that perform display, interaction, processing, control, and other functions through signals or instructions.

[0201] The memory stores at least one instruction, which is loaded and executed by the processor to implement the motion planning method for a steam generator tube sheet quadruped robot.

[0202] It should be noted that the specific embodiments are merely explanations and illustrations of the technical solution of the present invention and should not be used to limit the scope of protection. Any modifications made in accordance with the claims and specification of the present invention that are only partial should still fall within the protection scope of the present invention.

Claims

1. A motion planning method for a quadruped robot on a steam generator tube sheet, characterized in that, The quadruped robot comprises a base module, a working arm module, four leg modules, and four toe modules; Four leg modules are mounted on the base module; a passive rotational joint is set at the center of the base module. Each leg module includes a foot end and a rotational joint connecting the foot end to the base. The rotational joint connecting the foot end to the base is referred to as the first rotational joint. The foot has two rotational joints. The foot rotational joint that is closer to the first rotational joint is called the second rotational joint, and the other rotational joint is called the third rotational joint. These three rotational joints determine the robot's movement in the plane. The combined relative motion of the base module and the four leg modules is used to enable the robot to walk on the tube sheet; Each foot end is equipped with a movable joint, and the four toe modules are respectively set at the ends of the four leg modules through movable joints. The toe modules can be lifted and gripped to fix with the tube plate, and can be released and lowered to detach from the tube plate. The motion planning method includes the following steps: First, a traversal search is performed to determine the motion space of the base and the foot end, and to determine the set of feasible footing points: for a pipe hole, if the robot's forward and inverse kinematic conditions are met, the footing position does not block the pipe, and the foot does not collide with the evaporator water chamber after landing, then the point corresponding to the pipe hole position is a feasible footing point; by traversing the foot end work space, the set of all feasible footing points at that position can be obtained. The given polyline path is then divided into multiple straight paths. Based on each straight path, the base trajectory is planned, and the base position is determined. For each straight path segment, there is a starting point, an ending point, and a direction of movement. The robot selects a landing point from the feasible landing set based on the base's turning motion evaluation index and continuously performs the turning motion until the robot adjusts its posture to face the given orientation, completing the turning motion. The robot selects a landing point from the feasible landing set based on the straight motion evaluation index and continuously repeats the straight motion process until the robot base reaches the end of the straight path. This achieves the robot's movement on the given polyline path. The process by which the robot selects a landing point from the set of feasible landing points based on the base steering motion evaluation index includes the following steps: A1. Define the evaluation index of the robot's base steering motion based on the robot's initial motion posture: 1) Minimal Indicator 1: The angle between the aforementioned orientation and the given direction; 2) Miniature specification 2: The difference between the angle of the foot relative to the base and the standard angle of 45°; 3) Extremely large index 3: The size of the set of feasible landing points after landing; A2. Based on the evaluation index of base turning motion, plan the base turning motion of the robot: Use the TOPSIS method to perform positive and standardization processing on the three evaluation indexes of turning motion respectively, and find the optimal and worst solutions of the three indexes. According to the weight of the three base turning motion evaluation indexes, calculate the turning motion score of each landing point, and select the landing point with the highest score to land and realize the turning motion. The process by which the robot selects a landing point from the set of feasible landing points based on linear motion evaluation metrics includes the following steps: B1. Analyze the linear motion form of the robot and define the evaluation index of the robot's linear motion: a) Minimal indicator a: The distance the foot point moves along a given path; b) Minimal indicator b: The difference between the angle of the foot relative to the base and the standard angle of 45°; c) Minimalism indicator c: The distance between the robot's center of gravity and the direction of movement; d) Extremely large index d: The size of the set of feasible landing points after landing; B2. Based on the linear motion evaluation indicators, plan the robot's linear motion: Using the TOPSIS method, the four evaluation indicators of linear motion are positiveized and standardized respectively, and the optimal and worst solutions of the four indicators are obtained. Based on the weights of the four linear motion evaluation indicators, the linear motion score of each landing point is calculated, and the landing point with the highest score is selected to achieve linear motion.

2. The motion planning method for a quadruped robot on a steam generator tube sheet according to claim 1, characterized in that, In the process of finding all feasible landing points, the robot's forward kinematics conditions are satisfied, which means the following forward kinematics solution is satisfied: For a square tube sheet, the robot base position is as follows: ,in , The row and column coordinates of the heat transfer tubes on the square tube sheet corresponding to the robot base are given relative to the plane of the tube sheet; the positions of each foot are obtained. ): ， in, Indicate each foot The angle of rotation of the shaft connected to the machine body relative to the fixed tube sheet. ; This is the distance between the rotation axes of the first and second rotary joints. The distance between the rotation axis of the third rotary joint and the axis of the locating joint is actually the length of the two forearms at the foot end of each leg module; b and w represent 1 / 2 of the distance between the rotation axes of two adjacent first rotary joints.

3. The motion planning method for a quadruped robot on a steam generator tube sheet according to claim 2, characterized in that, In the process of finding all feasible landing points, the inverse kinematics conditions of the robot are satisfied, which means the following inverse kinematics solution is satisfied: The positions of the three support legs relative to the base are as follows: and The rotation angles of the two rotational joints of foot j are obtained. and ; ， If there are solutions for the two turns of foot j, then this is a feasible inverse kinematics solution / solution pair.

4. The motion planning method for a quadruped robot on a steam generator tube sheet according to claim 3, characterized in that, During the process of traversing and searching to determine the motion space of the base and the feet, a method for regularizing the robot's motion space is adopted. The method for regularizing the robot's motion space is as follows: Based on the position and orientation of the base, and according to the settings and movement range of the four leg modules of the quadruped robot, the position and orientation range of each foot is determined as a truncated annular region; at the same time, based on the position of the four feet of the robot, the actual movement space of the base is determined as the region where the annular regions corresponding to the position and orientation ranges of the four feet or the annular regions corresponding to the position and orientation ranges of the three feet intersect. The motion space is processed using the envelope method. Considering the distribution of tube holes in the tube sheet, the minimum envelope rectangle of the base is used to replace the base motion space, and the minimum envelope rectangle of the foot end is used to replace the foot motion space. The minimum envelope rectangle is the rectangle with the smallest area that can completely contain the foot or base motion space.

5. The motion planning method for a quadruped robot with a steam generator tube sheet according to claim 4, characterized in that, The process of planning the base trajectory and determining the base location based on each straight path includes the following steps: 3.1 Solving for the optimal ideal base position using the Lagrange multiplier method: Let the starting position of the base be... The endpoint of the direction of movement The coordinates of the two corner points of the rectangle encompassing the current support foot are: and Then the optimal ideal base position The optimization problem is described as follows: ， Subject to ， ， ， ， in, Represents the L2 norm, ; This ensures that the x-coordinate of the optimal ideal base position is within a rectangular range; these four constraints and the optimization function ensure that the endpoint of the base trajectory is closest to the direction of motion, and the optimal solution is that the endpoint is on a straight line in the direction of motion. Introducing Lagrange multipliers The corresponding Lagrange function is obtained as follows: ， The solution set for the ideal base position is obtained by introducing the KKT conditions. By using the distance from the endpoint of the linear motion as the evaluation index, the optimal ideal base position can be obtained. ; 3.2 Solving for the actual base trajectory: exist Nearby, with the requirement of satisfying the feasible solution of inverse kinematics, a traversal search is performed to obtain the feasible base positions along the motion direction under each attitude.

6. The motion planning method for a quadruped robot on a steam generator tube sheet according to claim 5, characterized in that, The solution set for the ideal base position is obtained by introducing the KKT conditions. The KKT conditions mentioned in the process are as follows: in, express exist Partial derivative of direction.

7. A motion planning method for a quadruped robot for a steam generator tube sheet according to any one of claims 1 to 6, characterized in that, The specific process of planning the robot's base steering motion based on the base steering motion evaluation index, as described in A2, includes the following steps: The left front leg of the robot body is defined as the lf leg, the left rear leg as the lb leg, the right front leg as the rf leg, and the right rear leg as the rb leg. During movement, each leg lands in the gait sequence of lf-rf-rb-lb. Before each landing, the base needs to be adjusted with the support of the other three legs. Turning movement only considers rotating the base, and the goal is to rotate to the target angle within the movement space of the base. The steps for calculating the steering motion score are as follows: A201, Data Forwarding: The p-th feasible landing point is evaluated, where q represents the index of the maximum / minimum index. This refers to the raw data, i.e., the actual values ​​of each indicator. This is the data obtained after forward processing; Positiveing ​​of extremely small indicators: , Positive transformation of extremely large indicators: , A202. Standardize all positive indicators, and record the standardized data as follows: : , A203. Find the optimal and worst solutions: Optimal solution vector: , worst solution vector: , Where p is the calculated landing point number. The number of all feasible landing points; A204. Calculate the turning motion score for each landing point. : ， in, The weights of each indicator; A205. Based on the score ranking, select the highest-scoring footing point to land and achieve the turning motion.

8. The motion planning method for a quadruped robot with a steam generator tube sheet according to claim 7, characterized in that, The highest score landing point is .

9. A computer storage medium, characterized in that, The storage medium stores at least one instruction, which is loaded and executed by a processor to implement a motion planning method for a steam generator tube sheet quadruped robot as described in any one of claims 1 to 8.

10. A motion planning device for a quadruped robot on a steam generator tube sheet, characterized in that, The device includes a processor and a memory, the memory storing at least one instruction, which is loaded and executed by the processor to implement a motion planning method for a steam generator tube sheet quadruped robot as described in any one of claims 1 to 8.

Images