Path planning method of positioning robot based on SG heat transfer tube detection

Abstract

The invention belongs to the technical field of path planning in navigation, and particularly relates to a path planning method of a positioning robot based on SG heat transfer tube detection. Inputting a heat transfer tube panel environment matrix A, a robot starting center point S and a detection point E; performing position transformation on the detection point E of the specific area which cannot be reached; determining coordinates of a central point and four feet at the detection point El under different postures; according to the pre-path and the posture of the robot at the E0, the starting point and the posture of each section are reversely deduced, and the feasibility of walking in the pre-path by using the postures is judged; if so, storing the path and state data; otherwise, searching ferry points with as few turning points as possible and as short distance as possible, and giving paths and postures of all sections before and after the ferry points; and performing repeated point elimination on the key point data and attitude of each path, and storing the simplified key point data and attitude. According to the invention, the planned path has few turning points, the state change frequency of the positioning robot is few, and the positioning robot can realize rapid displacement from the initial position to the detection point.

Description

Technical Field

[0001] The present invention belongs to the technical field of path planning in navigation, and particularly relates to a path planning method for a positioning robot based on SG heat transfer tube detection. Background Art

[0002] The performance of the SG heat transfer tubes in a nuclear power plant directly affects the safety of nuclear facilities. It is necessary to regularly detect and repair them to avoid radioactive material leakage caused by equipment damage, which poses a threat to the safety of the surrounding environment and personnel.

[0003] Currently, a positioning robot is used for the detection of SG heat transfer tubes in a nuclear power plant, and its path planning adopts the A* algorithm. As is well known, the path planning of the classic A* algorithm searches for a path with the shortest Manhattan distance between two nodes as the optimal principle, and the path search starts from the starting point to the end point. For the improved bidirectional A* algorithm, it starts searching from the starting point and the end point respectively. When the same current node is searched, the search ends. Although the search efficiency of the algorithm is improved and the search time is shortened compared with the classic A* algorithm, these algorithms cannot be directly applied to the path planning of the positioning robot for SG heat transfer tube detection.

[0004] The path planning of the positioning robot for SG heat transfer tube detection has more requirements for path selection: the Manhattan distance of the path where the robot center moves is the smallest, the number of turning points on the path where the robot walks is the least, the tube holes around the path can ensure that all four feet of the robot have attachment during walking, the fuselage or detector of the robot will not hit the wall during the state change process, and the robot can effectively avoid obstacles using obstacle information. It can be seen that the path planning of the positioning robot for SG heat transfer tube detection is not a problem that can be simply solved by applying the A* algorithm. Summary of the Invention

[0005] The purpose of the present invention is to provide a path planning method for a positioning robot based on SG heat transfer tube detection. In this method, the SG heat transfer tube panel environment matrix A, the starting center point S of the robot, and the detection point E are input; the position transformation of the detection point E in a specific area that cannot be reached is performed; the possible postures at the detection point E in A and the coordinates of the center point and four feet in different postures are determined; for vertical tube holes (or triangular tube holes), the center position E0 and posture of the robot at the detection point are determined according to the minimum angle between the line connecting the walking foot to the interface center (or starting point); then, the pre-path is obtained and the posture of the starting point is deduced from the destination, and the feasibility of the pre-path is verified according to each section of the posture. If it is feasible, the path with the fewest turning points and the shortest distance and the corresponding state are found; otherwise, a ferry point with as few turning points as possible and as short a distance as possible is searched within a certain range, and the paths and postures of each section before and after the ferry point are given.

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

[0007] A path planning method for a positioning robot based on the detection of SG heat transfer tubes

[0008] Step 1) Input the SG heat transfer tube panel environment matrix A, the starting center point S of the robot, and the detection point E;

[0009] Step 2) Perform a position transformation on the detection point E in the specific area that cannot be reached, El = PosiTransform(E);

[0010] Step 3) Determine the possible postures at the detection point El in A;

[0011] Step 4) Determine the coordinates of the center point and the four feet at different postures at the detection point El;

[0012] Step 5) Based on the minimum angle between the walking foot and the line connecting to the interface center or the starting point, determine the center position E0 and the posture of the robot at the detection point El;

[0013] Step 6) According to A, S, and E0, determine the pre-path from S to E0;

[0014] Step 7) According to the pre-path, and the robot posture at E0 and E0, reverse-deduce the starting point and the postures of each section, and determine the feasibility of walking with these postures on this pre-path;

[0015] Step 8) If feasible, save the path and status data; otherwise

[0016] Step 9) Find a ferry point with as few turning points as possible and as short a distance as possible, and give the paths and postures of each section before and after the ferry point;

[0017] Step 10) Eliminate duplicate points from the key point data and postures of each section of the path, and save the refined key point data and postures.

[0018] Step 5) For vertical tube holes or triangular tube holes, based on the minimum angle between the walking foot and the line connecting to the interface center or the starting point, determine the center position E0 and the posture of the robot at the detection point El.

[0019] Step 10) Perform duplicate point elimination and end-point redundant transformation suppression processing on the key point data and postures of each section of the path, and save the refined key point data and postures.

[0020] The beneficial effects obtained by the present invention are:

[0021] The path planned by the present invention has few turning points, the number of state changes of the positioning robot is small, the path distance can usually reach the shortest Manhattan distance, and the four-foot attachment is guaranteed. Therefore, the positioning robot can achieve a rapid shift from the initial position to the detection point. The path and its attitude obtained by reverse deduction from the end point to the starting point can effectively improve the effectiveness of the path and reduce the probability of the positioning robot hitting the wall at the back end of the path. The ferry technology adopted solves the problems of non-direct reach of the edge points and obstacle blocking of the pre-path. Description of the Drawings

[0022] Figure 1 Flowchart of the technical solution;

[0023] Figure 2 Main function SimpleClimbing.m;

[0024] Figure 3 Eight postures suitable for the panel positioning robot;

[0025] Figure 4 Path and attitude with the detection position being at the right center of the tube sheet;

[0026] Figure 5 Path and attitude with the detection position being at the bottom of the tube sheet;

[0027] Figure 6 Path and attitude with the detection position being on the left lower edge of the tube sheet;

[0028] Figure 7 Path and attitude with the detection position being at the upper left corner of the tube sheet;

[0029] Figure 8 Path and attitude with the detection position being at the top middle of the tube sheet;

[0030] Figure 9 Path and attitude with the detection position being at the upper right of the tube sheet. Detailed Implementation Manner

[0031] The present invention will be described in detail below with reference to the drawings and specific embodiments.

[0032] Step 1) Input the SG heat transfer tube panel environment matrix A, the starting center point S of the robot, and the detection point E;

[0033] Step 2) Perform a position transformation on the detection point E in the specific area that cannot be reached, El = PosiTransform(E);

[0034] Step 3) Determine the possible postures at the detection point El in A;

[0035] Step 4) Determine the coordinates of the center point and the four feet in different postures at the detection point El;

[0036] Step 5) For vertical or triangular tube holes, determine the central position E0 and attitude of the robot at the detection point El based on the minimum angle between the walking leg and the line connecting to the interface center or the starting point.

[0037] Step 6) Determine the pre-path PrePath from S to E0 based on A, S, and E0.

[0038] Step 7) Based on the pre-path PrePath, as well as the position E0 and the robot attitude at E0, reverse-infer the starting point and the attitudes of each segment, and determine the feasibility of walking along this pre-path using these attitudes.

[0039] Step 8) According to the feasibility metric, if it is feasible, save the path and status data; otherwise

[0040] Step 9) Search for ferry points within a certain range with as few turning points as possible and as short a distance as possible, and give the paths and attitudes of each segment before and after the ferry points.

[0041] Step 10) Eliminate duplicate points and suppress redundant endpoint transformations for the key point data and attitudes of each segment of the path, and save the refined key point data and attitudes.

[0042] First, download the SG heat transfer tube interface data A, display this interface, and input the initial position S of the positioning robot and the point E to be detected on the interface. Through the function ClimberTrack.m designed by this method, calculate the paths and attitudes of the key points on the path, and use the matrix expansion method to obtain the information of all points on the path, so as to display the full path and attitude. Here, it is denoted by the main function SimpleClimbing.m.

[0043] The function ClimberTrack.m embodies the core technology. This function contains multiple functional sub-functions, such as: PosiTransform.m, TurnPoint.m, Stateof5piontNew.m, stateChange.m, ShortDistFewPose.m, SGPoseNew.m, SGPoseDetSpot.m, SearchState.m, RouteStateV.m, RotateTimes.m, PointPostNew.m, Flagpp.m, Flagp.m, distw.m, deleteSameR.m, C2ProbeStateof5piontNew.m, baidu.m, allpose.m. Their respective functions are as follows:

[0044] PosiTransform.m: Perform position transformation on the detection point E in a specific area that cannot be reached.

[0045] TurnPoint.m: Take a turning point on a straight line or a polyline, take an inflection point on a polyline, take an intermediate point on a straight line, and the position value of the point is consistent with the actual drawing regulations.

[0046] Stateof5piontNew.m: Determine the coordinate positions of 5 points of a specific robot according to the input number of states and the positions of detection points: the center point and 4 surrounding points.

[0047] stateChange.m: Rotate according to the original state to determine the new state.

[0048] ShortDistFewPose.m: Determine the path with the fewest inflection points and the shortest Manhattan distance and the path length between the starting point and the ending point according to the tube sheet digital matrix and its starting point and ending point.

[0049] SGPoseNew.m: Take each point of the tube sheet as the center point of the positioning robot and find out the number of poses of each center point.

[0050] PointPostNew.m: Find out the possible states and their quantities at the detection point.

[0051] SGPoseDetSpot.m: Find out the state matrix of each point of the SG heat transfer tube tube sheet as the detection point.

[0052] SearchState.m: Given the subsequent state and the paths before and after, deduce the previous state backwards.

[0053] RouteStateV.m: Given the robot state and the path, find the feasibility of each point on the path.

[0054] RotateTimes.m: Determine the number of rotations required to reach the state near the most terminal according to the path terminal and the terminal locking pose.

[0055] Flagp.m: Path validity judgment when there are no inflection points.

[0056] Flagpp.m: Path validity judgment when there are inflection points.

[0057] distw.m: Euclidean distance between two points.

[0058] deleteSameR.m: Eliminate the same rows and streamline the repeated process.

[0059] C2ProbeStateof5piontNew.m: Given the robot center, find the coordinates of the probe and the four legs.

[0060] baidu.m: Search for ferry points.

[0061] allpose.m: Re - determine the path and the robot pose according to the ferry point.

[0062] By calling the above - mentioned sub - functions, the core function ClimberTrack.m realizes the path planning and pose determination from the starting point to the detection point.

[0063] The specific process of the core function of the embodiment is as follows:

[0064] Step 1) Call the SG heat transfer tube panel environment matrix A, the starting center point S of the robot, and the detection point E.

[0065] Step 2) Perform a position transformation on the detection point E in the specific area that cannot be reached, El = PosiTransform(E);

[0066] Step 3) Determine the possible poses and their numbers at the detection point El, [FS,F] = PointPostNew(A,El);

[0067] Step 4) Use Stateof5piontNew to find the coordinates of the center point and the four legs at different poses at the point El.

[0068] Step 5) For vertical tube holes, determine the center position E0 and the pose of the robot at the detection point El according to the minimum angle between the connecting lines of the two walking legs to the center of the interface semi - circle.

[0069] Step 6) According to A, S, and E0, use ShortDistFewPose to determine the pre - path PrePath from S to E0.

[0070] Step 7) According to the pre - path PrePath, and the robot pose at E0 and E0, reverse - deduce the starting point and each section of the pose, and determine the feasibility of walking with these poses on this pre - path, represented by the flag logical value;

[0071] Step 8) According to flag = 0, if it is feasible, save the path and status data; otherwise

[0072] Step 9) Search for a ferry point Baidu with as few turning points as possible and as short a distance as possible within a certain range, and give the paths and poses of each section before and after the ferry point. The paths and poses are represented by the matrix AllPosipose;

[0073] Step 10) Eliminate duplicate points and suppress the redundant transformation of the end points for the key point data and poses of each section of the path, and save the refined key point data and poses, still denoted as AllPosipose.

[0074] To determine the state and its rotation, 8 poses suitable for positioning the robot on this panel are defined asFigure 3 As shown (here are 7 points: the green circle in the middle represents the center of the robot, the green circles on both sides represent the walking feet, the yellow circles represent the two sliding feet, and the red * represents the outermost probe).

[0075] The following are the initial positions S of 6 representative positioning robots and the points E to be detected after running the main function, obtaining the feasible paths and postures.

[0076] The detection point selected for the first time is at a position to the right of the center of the tube sheet, as shown in Figure 4 (a). Although the detection point is at the center position, an obstacle is still encountered. At the turning point, one sliding foot presses against the blind hole, as shown in Figure 4 (b). It can be seen that through ferrying and feasibility determination, the feasible path and corresponding posture of the positioning robot walking from the starting point to the detection point are obtained, as shown in Figure 4 (c).

[0077] The detection point selected for the second time is at the bottommost, the last point in the last section, and the positioning robot can reach it by sliding, as shown in Figure 5 .

[0078] The detection point selected for the third time is on the left lower edge. It can be seen that the algorithm directly obtains the determined path and posture, as shown in Figure 6 .

[0079] The detection point selected for the fourth time is in the upper left corner. It can be seen that the algorithm directly obtains the determined path and posture. At the last three points in the last section, it must slide in the original state, as shown in Figure 7 .

[0080] The detection point selected for the fifth time is at the top in the middle. It can be seen that the algorithm directly obtains the determined path and posture, as shown in Figure 8 .

[0081] The detection point selected for the sixth time is in the upper right. It can be seen that the algorithm obtains the determined path and posture. In the later part, it cannot directly reach the edge on the upper side and needs to slide 1 unit. Finally, it must first rotate 1 state downward and then walk 3 units downward, as shown in Figure 9 .

Claims

1. A path planning method for a positioning robot based on SG heat transfer tube detection, characterized in that: Step 1) Input the SG heat transfer tube panel environment matrix A, the starting center point S of the robot, and the detection point E; Step 2) Perform a position transformation on the detection point E in a specific area that cannot be reached, El = PosiTransform(E); Step 3) Determine the possible postures at the detection point El in A; Step 4) Determine the coordinates of the center point and the four feet under different postures at the detection point El; Step 5) Based on the minimum angle between the walking foot and the line connecting to the interface center or the starting point, determine the center position E0 and the posture of the robot at the detection point El; Step 6) According to A, S, and E0, determine the pre-path from S to E0; Step 7) According to the pre-path, and the robot posture at E0 and E0, reverse-deduce the starting point and the postures of each segment, and determine the feasibility of walking with these postures on this pre-path; Step 8) If feasible, save the path and status data; Otherwise Step 9) Find a ferry point with as few turning points as possible and as short a distance as possible, and give the paths and postures of each segment before and after the ferry point; Step 10) Eliminate duplicate points from the key point data and postures of each segment of the path, and save the refined key point data and postures.

2. The path planning method for a positioning robot based on SG heat transfer tube detection according to claim 1, characterized in that: Step 5) For vertical tube holes or triangular tube holes, based on the minimum angle between the walking foot and the line connecting to the interface center or the starting point, determine the center position E0 and the posture of the robot at the detection point El.

3. The path planning method for a positioning robot based on SG heat transfer tube detection according to claim 1, characterized in that: Step 10) Perform duplicate point elimination and end-point redundancy transformation suppression processing on the key point data and postures of each segment of the path, and save the refined key point data and postures.

Images