Fast collision detection method, system and device for heat pipe maintenance robot

Abstract

The application discloses a quick collision detection method, system and equipment of a heat transfer pipe maintenance robot, and belongs to the technical field of robot collision detection. In order to solve the problem that the existing collision detection method of the heat transfer pipe maintenance robot cannot well balance the calculation speed on the basis of ensuring the accuracy of collision detection, the application first projects each component of the robot on a horizontal plane in two dimensions according to the structure of the heat transfer pipe maintenance robot and equivalently forms geometric figures; then, taking the center of the geometric figure corresponding to the base of the robot as an origin, the robot is divided into four quadrants, the quadrants in which the geometric figures are located are determined through position coordinates, collision calculation of the geometric figures is performed in the corresponding quadrants, and internal collision detection is completed; whether the OBB (Oriented Bounding Box) enclosing box corresponding to the two-dimensional projection figure set is in the collision semicircle of the operating arm is checked, if yes, it is indicated that the robot has no collision with the external environment, and if not, whether the geometric figures of the components are in the collision semicircle of the corresponding height is determined, and if yes, it is determined that there is no collision.

Description

Technical Field

[0001] This invention belongs to the field of robot collision detection technology, and relates to a rapid collision detection method, system and equipment for a heat transfer pipe maintenance robot. Background Technology

[0002] The steam generator in a nuclear power plant, as a key heat exchange device between the nuclear reactor and the turbine generator, plays a vital role in the nuclear energy field. Heat transfer tubes, as a core component of the steam generator, are responsible for transferring heat from the high-temperature, high-pressure reactor coolant to the secondary circulating water, thereby generating steam to drive the turbine generator. Considering the importance of heat transfer efficiency, the tube walls are typically designed to be relatively thin. However, heat transfer tubes operate in harsh environments, under high temperature and pressure and exposed to nuclear radiation, making them susceptible to damage, which in turn affects the safety and performance of the nuclear power plant.

[0003] To ensure the safe operation and performance of heat transfer tubes, remote robotic control is commonly used for their inspection and maintenance. Tube sheet crawling inspection robots are one widely adopted type of inspection robot. However, these robots are limited by their range of motion and space during autonomous or manual operation, especially in the edge areas of the heat transfer tubes where the movement space is extremely limited. During robot operation, it is crucial to ensure that internal components do not collide with each other, and that the robot itself does not collide with the external environment, while maintaining both accuracy and computational efficiency. To meet the requirements of real-time robot operation planning, the collision detection algorithm must possess high-speed computational capabilities to ensure efficient movement. Therefore, a collision detection algorithm capable of high-speed and accurate collision calculation is needed. Summary of the Invention

[0004] This invention addresses the problem that existing collision detection methods for heat transfer tube maintenance robots cannot adequately balance collision detection accuracy with computational speed.

[0005] A rapid collision detection method for a heat transfer pipe maintenance robot, wherein the heat transfer pipe maintenance robot structure includes a mobile body and an operating arm; the mobile body includes a base and multiple legs symmetrically arranged on the base; each leg includes a thigh and a calf, the thigh is connected to the base via a first leg joint, and the calf is connected to the thigh via a second leg joint, the first leg joint and the second leg joint being rotary joints; the operating arm includes an upper arm and a forearm, the upper arm is connected to the center of the base via a first arm joint, and the forearm is connected to the upper arm via a second arm joint, the first arm joint and the second arm joint being rotary joints; a probe is located at the end of the forearm; the method includes the following steps:

[0006] First, based on the structure of the heat transfer pipe maintenance robot, each component of the robot is projected onto a horizontal plane in two dimensions and represented as an equivalent geometric shape: the base is equivalent to a rectangle; the rotational joint connecting the thigh to the base is equivalent to a circle, and the equivalent geometric shape of the thigh includes: the thigh torso portion is equivalent to a rectangle, and the rotational joint between the thigh and the lower leg is equivalent to a circle; the equivalent geometric shape of the lower leg includes: the lower leg torso portion is equivalent to a rectangle, and the extension joint at the end of the lower leg is equivalent to a circle; the rotational joint connecting the upper arm to the base is equivalent to a circle, and the equivalent geometric shape of the upper arm includes: the upper arm torso portion is equivalent to a rectangle, and the rotational joint connecting the forearm to the upper arm is equivalent to a circle; the equivalent geometric shape of the forearm includes: the forearm torso portion is equivalent to a rectangle, and the end of the forearm is equivalent to a circle; the probe at the end of the forearm is equivalent to a rectangle.

[0007] Then, internal collision detection and external collision detection are performed based on equivalent geometry;

[0008] The internal collision detection based on equivalent geometry includes: using the center of the geometry corresponding to the robot base as the origin and the two mutually perpendicular axes of symmetry of the robot base as the x and y coordinate axes, directly dividing the robot into four quadrants, and calculating the absolute position of the geometric shape corresponding to the projection of each component in the two-dimensional projection geometry set of the robot in this coordinate system. The quadrant of each geometric shape is determined by the absolute position coordinates, and the geometry collision calculation is performed in the corresponding quadrant. If the shape intersects with the coordinate axis, the corresponding shape is placed in two adjacent quadrants, and the geometry collision calculation is performed in each quadrant. The geometry collision calculation determines whether the geometric shapes corresponding to different components intersect; if they intersect, it is a collision; otherwise, it is not a collision.

[0009] The external collision detection based on equivalent geometry includes:

[0010] The external environment consists of a quarter-spherical shell, a semi-circular partition, and a tube sheet; the robot works within the quarter-sphere enclosed by these three elements.

[0011] For a moving body, the collision between a robot of height h and a spherical shell of radius r is transformed into the collision of the robot's highest cross section with radius sqrt(r). 2 -h 2 The collision of a semicircular arc of the body is called the body collision semicircle.

[0012] For the robot's manipulator arm and probe, the collision section heights of the manipulator arm and probe correspond to the heights of the top surface of the manipulator arm and the probe, respectively, and the corresponding semicircles are called the manipulator arm collision semicircle and the probe collision semicircle, respectively.

[0013] First, perform preliminary external detection: generate an OBB bounding box for the robot's 2D projection graphic set, and check whether the generated bounding box is within the manipulator collision semicircle; if the bounding box is contained within the manipulator collision semicircle, it means that the robot has no collision with the external environment; otherwise, perform precise external detection.

[0014] Precise external detection: Determine whether the leg circle in the equivalent geometry of the thigh and the leg circle in the equivalent geometry of the calf are within the body collision semicircle. If they are, no collision is determined. Determine whether the upper arm circle in the equivalent geometry of the upper arm is within the operating arm collision semicircle. If they are, no collision is determined. At the same time, determine whether the forearm circle in the equivalent geometry of the forearm and the rectangle corresponding to the probe at the end of the forearm are within the probe collision semicircle. If they are, no collision is determined.

[0015] Furthermore, before performing internal collision detection, each leg and the manipulator can be regarded as a two-degree-of-freedom chain structure. Collisions are avoided within the single chain corresponding to the manipulator or each leg by limiting the range of joint angles. When performing internal collision detection based on equivalent geometry, collision detection of the geometry corresponding to the single chain of the manipulator or each leg is excluded.

[0016] Furthermore, when starting internal collision detection based on equivalent geometry, a quadtree collision detection space segmentation method is first used to exclude parts in the robot space that will not collide with each other, and then internal collision detection is performed based on equivalent geometry.

[0017] Furthermore, in the process of calculating the absolute position of the geometric figure corresponding to the projection of each component in the two-dimensional projection graphic set of the robot in the coordinate system, based on the positive kinematic equations of the robot's legs and manipulator, and through the given joint angles and structural dimensions of each component of the robot, the absolute position of the projection of each component in the two-dimensional projection graphic set of the robot in the coordinate system is calculated.

[0018] The forward kinematic equation of the leg is as follows:

[0019]

[0020] Where i represents the i-th leg; θ i1 Let θ be the rotation angle of the first leg joint of leg i. i2 Let be the rotation angle of the second leg joint of leg i; L1 is the thigh length, L2 is the calf length; d2 is the distance between the first and second leg joints on each leg on the Z-axis in the world coordinate system; d4 is the distance between the telescoping joint and the foot end on the Z-axis in the world coordinate system; d i3 Let be the distance the extension joint of leg i moves.

[0021] Furthermore, the process of determining whether the geometric figures corresponding to different components intersect includes:

[0022] (1) Determining the intersection of two circles:

[0023] For two circles, denoted as circle A and circle B, which are equivalent to circles in a two-dimensional projected graphic space, let their centers be (x_1, y_1) and (x_2, y_2) respectively, and their radii be r_1 and r_2 respectively; the intersection problem is transformed into the following equation:

[0024]

[0025] (2) Determining the intersection of a circle and a rectangle:

[0026] Given an equivalent circle and a rectangle in a two-dimensional projected graphic space, denoted as circle M and rectangle CDEF, with the center M of the circle having coordinates (x_0, y_0) and radius r_0, and the four vertices CDEF of the rectangle having coordinates (c_1, d_1), (c_2, d_2), (c_3, d_3), and (c_4, d_4), solve for the intersection relationship between the circle and the rectangle.

[0027] First, translate the center of the rectangle to the origin of the established rectangular coordinate system, keeping the relative positions of the rectangle and the circle fixed, and the circle translates accordingly. Then, rotate the rectangle so that one side is parallel to the fixed coordinate system, keeping the relative positions fixed, and the circle rotates accordingly as well. At this point, the center of the circle may be in any quadrant. Take the absolute values ​​of the center coordinates; the circle is now mirrored to the first quadrant. Assume the vector from the corner of the rectangle in the first quadrant to the center of the circle... At this time, the vector There are four possibilities:

[0028] In case (1), x>0 and y>0, at this time

[0029] In case (2), x < 0 and y > 0, then u = y;

[0030] In case (3), x>0 and y<0, then u=x;

[0031] In case (4), x < 0 and y < 0, then u = 0;

[0032] In four cases, when the radius r > u, the rectangle and the circle do not intersect.

[0033] (3) Determining the intersection of rectangles:

[0034] For two equivalent rectangles in a two-dimensional projected graphic space, denoted as rectangle HIJK and QRPT, let the two rectangles be rectangle H and rectangle N respectively; given the vertex coordinates of the two rectangles as (f_1,g_1), (f_2,g_2), (f_3,g_3), (f_4,g_4) and (m_1,n_1), (m_2,n_2), (m_3,n_3), (m_4,n_4), solve the collision relationship of the rectangles. The specific calculation steps are as follows;

[0035] First, translate the center of rectangle H to the origin, keeping the relative position unchanged, and also translate rectangle N accordingly. Then, rotate rectangle H around the origin until one side is parallel to the coordinate axes, keeping the relative position unchanged, and also rotate rectangle N accordingly, resulting in the transformed coordinates of the two rectangles. At this point, rectangle H has two different x-coordinate values ​​at its four corners, and rectangle N has four different x-coordinate values. Take the interval formed by the maximum and minimum values ​​of the four coordinate values ​​of rectangle N and determine if it overlaps with the interval formed by two coordinate values ​​of rectangle H. Continue this process to determine the y-axis. At this point, the determination of whether there is a separating axis when the rectangle is parallel to two sides of rectangle H has been completed. If there is a non-overlapping coordinate interval between the x and y axes, the two rectangles do not intersect. If there is an overlap, rectangle N needs to be moved to the origin, and the above operation repeated to complete the determination of parallelism to the four axes of the two rectangles.

[0036] Furthermore, the process of generating the OBB bounding box of the robot's two-dimensional projected graphic set includes:

[0037] The robot's 2D projection graphic set includes rectangles and circles. Feature points of all geometric shapes are extracted. For rectangles, four vertices are selected as feature points. For circles, the center is selected as the feature point. An initial OBB bounding box is generated using all feature points. This bounding box is then expanded outward by the radius of the leg circle to obtain the final OBB bounding box.

[0038] Furthermore, during the precise external detection process, it is also determined whether the rectangle equivalent to the base is within the collision semicircle of the main body, and if it is within the collision semicircle of the main body, it is determined that there is no collision.

[0039] A rapid collision detection system for a heat transfer tube maintenance robot includes:

[0040] Equivalent geometric units for the robot: Based on the structure of the heat transfer pipe maintenance robot, each component of the robot is projected onto a horizontal plane in two dimensions and represented as an equivalent geometric shape: the base is equivalent to a rectangle; the rotational joint connecting the thigh to the base is equivalent to a circle; the equivalent geometric shape of the thigh includes: the thigh torso portion is equivalent to a rectangle, and the rotational joint between the thigh and the lower leg is equivalent to a circle; the equivalent geometric shape of the lower leg includes: the lower leg torso portion is equivalent to a rectangle, and the extension joint at the end of the lower leg is equivalent to a circle; the rotational joint connecting the upper arm to the base is equivalent to a circle; the equivalent geometric shape of the upper arm includes: the upper arm torso portion is equivalent to a rectangle, and the rotational joint connecting the forearm to the upper arm is equivalent to a circle; the equivalent geometric shape of the forearm includes: the forearm torso portion is equivalent to a rectangle, and the end of the forearm is equivalent to a circle; the probe at the end of the forearm is equivalent to a rectangle.

[0041] The heat transfer pipe maintenance robot structure includes a mobile body and an operating arm. The mobile body includes a base and multiple legs symmetrically arranged on the base. Each leg includes a thigh and a calf. The thigh is connected to the base via a first leg joint, and the calf is connected to the thigh via a second leg joint. The first and second leg joints are rotary joints. The operating arm includes a large arm and a forearm. The large arm is connected to the center of the base via a first arm joint, and the forearm is connected to the large arm via a second arm joint. The first and second arm joints are rotary joints. A probe is located at the end of the forearm.

[0042] An internal collision detection unit based on equivalent geometry is used: The robot is divided into four quadrants with the center of the geometry corresponding to the robot base as the origin and the two mutually perpendicular axes of symmetry of the robot base as the x and y coordinate axes. The absolute position of the geometric shape corresponding to each component in the two-dimensional projection set of the robot is calculated in this coordinate system. The quadrant of each geometric shape is determined by the absolute position coordinates, and collision calculation is performed in the corresponding quadrant. If a shape intersects with a coordinate axis, the corresponding shape is placed in two adjacent quadrants, and collision calculation is performed in each quadrant. The collision calculation determines whether the geometric shapes corresponding to different components intersect; if they intersect, it is a collision; otherwise, there is no collision.

[0043] The external collision detection unit based on equivalent geometry includes a collision semicircle analysis module, a preliminary external detection module, and a precise external detection module.

[0044] Collision semicircle analysis module: Determines the collision semicircles of the robot body, the manipulator arm, and the probe based on the robot's structure and external environment.

[0045] The external environment includes a quarter-spherical shell, a semi-circular partition, and a tube sheet; the robot works within the quarter-sphere enclosed by these three components.

[0046] For a moving body, the collision between a robot of height h and a spherical shell of radius r is transformed into the collision of the robot's highest cross section with radius sqrt(r). 2 -h 2 The collision of the semicircular arc of the robot is called the body collision semicircle; for the robot's manipulator arm and probe, the collision section heights of the manipulator arm and probe are respectively the heights of the top surface of the manipulator arm and the probe, and the corresponding semicircles are called the manipulator arm collision semicircle and the probe collision semicircle, respectively.

[0047] Preliminary external detection module: Generates the OBB bounding box of the robot's 2D projection graphic set, and checks whether the generated bounding box is within the manipulator collision semicircle; if the bounding box is contained within the manipulator collision semicircle, it means that the robot has no collision with the external environment.

[0048] Precise external detection module: Determines whether the leg circle in the equivalent geometry of the thigh and the leg circle in the equivalent geometry of the calf are within the body collision semicircle. If they are, no collision is determined. Determines whether the upper arm circle in the equivalent geometry of the upper arm is within the operating arm collision semicircle. If they are, no collision is determined. Simultaneously, it determines whether the forearm circle in the equivalent geometry of the forearm and the rectangle corresponding to the probe at the end of the forearm are within the probe collision semicircle. If they are, no collision is determined.

[0049] Furthermore, the precise external detection module is also used to determine whether the rectangle equivalent to the base is inside the collision semicircle of the body, and if it is inside the collision semicircle of the body, it is determined that there is no collision.

[0050] A rapid collision detection device for a heat transfer pipe maintenance robot, the device comprising a processor and a memory, the memory storing at least one instruction, the at least one instruction being loaded by the processor and used to execute the rapid collision detection system for the heat transfer pipe maintenance robot.

[0051] Beneficial effects:

[0052] This invention can effectively balance collision detection accuracy with computational speed. For a heat pipe maintenance robot, 1000 sets of different base poses and joint angle states are randomly generated within the range of motion of each joint. When using this invention for detection, an average computation time of 0.02 seconds can be achieved with an overall success rate of 99.6%. Attached Figure Description

[0053] Figure 1 A 3D structural diagram of the maintenance robot.

[0054] Figure 2 To establish the coordinate system of the maintenance robot

[0055] Figure 3 A comparison diagram of the projection of the 3D model of the maintenance robot and the set of 2D geometric figures.

[0056] Figure 4 This is a diagram illustrating how to determine the intersection of two circles.

[0057] Figure 5 This is a schematic diagram illustrating the calculation process for the intersection of a circle and a rectangle.

[0058] Figure 6 This is a diagram illustrating the criteria for determining the intersection of a circle and a rectangle.

[0059] Figure 7 This is a diagram illustrating how to determine the intersection of two rectangles.

[0060] Figure 8 Schematic diagram of quadrant partitioning of a quadtree

[0061] Figure 9 A schematic diagram showing the conversion of the three-dimensional external environment of the inspection robot into a two-dimensional form.

[0062] Figure 10 This is a schematic diagram of the enclosure for the maintenance robot.

[0063] Figure 11 This is a flowchart for collision detection of a maintenance robot.

[0064] Figure 12 Two-dimensional simulation of a set of experiments for the maintenance robot.

[0065] Figure 13 The three-dimensional simulation of a set of experiments for the maintenance robot. Detailed Implementation

[0066] The purpose of this invention is to provide a rapid collision detection method for a heat transfer tube maintenance robot. Given the tube sheet model, the robot's pose, and joint angles, the method calculates the collision situation of the robot's own components and the collision situation of the robot with the external evaporator shell and partition, while simultaneously meeting the requirements of accuracy and efficiency. The following is a detailed explanation... Figure 11 The specific implementation methods are described in detail. Specific implementation method one:

[0068] This embodiment is a rapid collision detection method for a heat transfer pipe maintenance robot, including the following steps:

[0069] Step 1: Based on the structure of the inspection robot, derive the robot's forward kinematics relationship and transform the 3D robot into a set of 2D geometric figures:

[0070] 1.1 Analyze the structural form of the robot

[0071] The quadrupedal maintenance robot can be divided into a mobile body and a manipulator arm 30, such as Figure 1 As shown. The mobile body includes a base 10 and four legs 20, each with three degrees of freedom. Each leg includes a thigh and a lower leg. The thigh is connected to the base through a first leg joint, and the lower leg is connected to the thigh through a second leg joint. The first and second leg joints are rotational joints. The lower leg ends with a foot end through a telescoping joint (linear translation joint), which enables the leg to be raised and lowered. The four legs have the same structure and are symmetrically distributed.

[0072] The operating arm 30 includes an upper arm and a lower arm. The upper arm is connected to the center of the base through a first arm joint, and the lower arm is connected to the upper arm through a second arm joint. The first arm joint and the second arm joint are rotary joints. There is a probe at the end of the lower arm.

[0073] The telescopic joints at the ends of the lower legs do not affect the robot's posture and can be ignored. The ten rotational joints of the manipulator and four legs limit the robot's posture. The robot's row position, column position, and rotation angle are three parameters that determine the robot's position.

[0074] 1.2 Derivation of the Forward Kinematics of the Robot

[0075] Using the robot's movement direction as a reference, the left front leg, left hind leg, right hind leg, and right front leg are named LF, LB, RB, and RF, respectively, and numbered 1, 2, 3, and 4. The four legs have identical structures and can be considered as a series linkage structure. Based on their distance from the robot's base, the three joints that enable spatial movement (first leg joint, second leg joint, and telescopic joint) are numbered 1, 2, and 3. The following coordinate system is established: Figure 2 As shown.

[0076] Define the volume coordinate system {O B It is fixed at the center of the base, and its coordinate system origin is O. B The three coordinate axes are X B Y B Z B Z B The axis direction is opposite to the direction of gravity, Y B The axis direction is opposite to the forward direction.

[0077] Define the world coordinate system as {O w It is fixed on the ground, and its coordinate system origin is O. w The three coordinate axes are X w Y w Z w Z w The axis direction is opposite to the direction of gravity, X w Axis, Y w The axial directions are respectively with X B Axis, Y BThe axes are parallel.

[0078] Define the joint coordinate system {O ij}, where i = 1, 2, 3, 4, representing the four legs LF, LB, RB, and RF respectively; j = 1, 2, 3, representing the three spatial movement joints (first leg joint, second leg joint, and extension joint). A coordinate system of 12 leg joints is established. {O i1}、{O i2}、{O i3 The centers of the circles are located at the first leg joint, the second leg joint, and the extension joint, respectively.

[0079] Define the foot coordinate system {O i4}, where i = 1, 2, 3, 4, representing the four legs LF, LB, RB, and RF respectively; {O i4 The origin of} is located at the tip of the foot;

[0080] Define the leg base coordinate system {O i0}, where i = 1, 2, 3, 4, representing the four legs LF, LB, RB, and RF respectively; {O i0 The origin of} is located on the first leg joint;

[0081] A total of four base coordinate systems and four foot coordinate systems are established for the four legs. Base coordinate system {O i0} is the fixed coordinate system, and {O} is the joint coordinate system. ij} and foot coordinate system {O i4 It can rotate with the movement of the legs, and the base coordinate system {O} i0} Used for joint coordinate system {O ij} and foot coordinate system {O i4 The coordinate system described above can actually be established in other ways, as long as it can achieve the transformation between different coordinate systems and coordinates.

[0082] Taking the left foreleg as an example, the improved DH parameter method is used to calculate the transformation matrix from the leg base coordinate system to the foot coordinate system of the left foreleg.

[0083]

[0084] Where, θ 11 θ is the rotation angle of the first joint of the left foreleg. 12 L1 is the rotation angle of the second leg joint of the left foreleg; L2 is the thigh length (the distance between the rotation axis of the first leg joint and the rotation axis of the second leg joint); L2 is the lower leg length (the distance between the rotation axis of the second leg joint and the axis of the telescoping joint); d2 is the length of the thigh on each leg. i1}、{O i2The distance from the origin on the Z-axis in the world coordinate system actually represents the distance between the first and second leg joints on the Z-axis in the world coordinate system; d4 is the distance between the origin and the second leg joint on each leg. i3}、{O i4 The distance between the origin and the Z-axis in the world coordinate system actually represents the distance between the telescopic joint and the foot tip on the Z-axis in the world coordinate system; d1 is the distance between the origin and the foot tip on each leg. i1}、{O B The distance between the origin and the Z-axis in the world coordinate system actually represents the distance between the first leg joint and the center of the base on the Z-axis in the world coordinate system; d 13 The distance traveled by the extension and retraction joint of the left foreleg;

[0085] From the transformation matrix The equation of motion for the left foreleg is obtained as follows. By determining the joint position, the coordinates of the foot tip relative to the leg's base coordinate system can be calculated:

[0086]

[0087] The robot's four legs have identical structures and are symmetrical about the center of the base. The forward kinematic equations for the four legs can be obtained as follows:

[0088]

[0089] Where i = 1, 2, 3, 4, representing the four legs LF, LB, RB, and RF respectively; since d is different from the fixed d2 and d4, d i3 They may be different, therefore use d i3 It means that d i3 That is, d 13 -d 43 .

[0090] For the manipulator, based on the lengths of the upper arm and forearm, the forward kinematic equations are established in the same way as those for the legs.

[0091] 1.3 Methods for converting 3D models into 2D geometric figures

[0092] Since this type of maintenance robot performs real-time planning, calculation, and manipulator movement within a horizontal plane, collision issues can be simplified by converting the complex 3D model collision problem into a geometric intersection problem within a 2D plane. This significantly reduces computation time and meets the demands of real-time operations. For the robot's moving body, a 2D projection can be obtained by projecting the 3D robot model onto a horizontal plane. This simplifies the 3D model collision problem into a 2D geometric intersection problem. Whether a 3D model collision occurs is determined by whether the 2D projections intersect.

[0093] For the robot's manipulator arm, based on the robot's structure, only the part of the upper arm that connects to the forearm is likely to collide with the legs. Since this collision area overlaps with the forearm, the upper arm can be considered as not colliding with the legs, and only the collision between the forearm and legs needs to be assessed. Therefore, the collision problem between the legs and the manipulator arm can be determined by whether the projections of the legs and the forearm in the perpendicular direction intersect. Figure 3 As shown, Figure 3 The left image shows the vertical projection of the robot's 3D model, while the right image shows the equivalent projection geometry on a 2D plane. Specifically, the base is equivalent to a rectangle (including rectangles of equal length and width, i.e., squares); the rotational joint connecting the thigh to the base (the first leg joint) is equivalent to a circle; the equivalent geometry of the thigh includes: the thigh torso portion is equivalent to a rectangle, and the rotational joint between the thigh and lower leg (the second leg joint) is equivalent to a circle; the equivalent geometry of the lower leg includes: the lower leg torso portion is equivalent to a rectangle, and the lower leg end extension joint is equivalent to a circle; the equivalent geometry of the upper arm includes: the rotational joint connecting the upper arm to the base (the first arm joint) is equivalent to a circle, the upper arm torso portion is equivalent to a rectangle, and the forearm to upper arm rotational joint (the second arm joint) is equivalent to a circle; the equivalent geometry of the forearm includes: the forearm torso portion is equivalent to a rectangle, and the forearm end is equivalent to a circle; the probe at the forearm end is equivalent to a rectangle.

[0094] By transforming the projected image into a set of simple two-dimensional geometric figures, the computational complexity can be further reduced and the detection speed improved. The collision status of the 3D robot can be determined by judging the intersection relationships of simple geometric figures, including circles and rectangles, within the two-dimensional geometric figures.

[0095] Step 2: Based on computer graphics knowledge, provide the criteria for determining the intersection of different figures:

[0096] Based on the robot's structure and dimensions, the set of two-dimensional geometric figures projected by the robot mainly consists of rectangles and circles, such as... Figure 3 As shown in the right figure, dealing with intersection problems mainly involves determining the intersection of circles, rectangles, and circles. The following provides solutions for these intersection problems.

[0097] (1) Methods for determining the intersection of two circles

[0098] Using geometric principles, the intersection of two circles can be easily determined by the relationship between their centers and radii, such as... Figure 4As shown. Taking arbitrary circles A and B in a two-dimensional projected graphic space as examples, let the centers of circles A and B be (x_1, y_1) and (x_2, y_2) respectively, and their radii be r_1 and r_2 respectively. The intersection problem is transformed into the following equation:

[0099]

[0100] (2) Methods for determining the intersection of a circle and a rectangle

[0101] like Figure 5 As shown, a rectangular coordinate system is set at the origin. Taking an arbitrary circle M (with the center M representing the circle) and a rectangle CDEF in a two-dimensional projected graphic space as an example, the coordinates of the center M of the circle are given as (x_0, y_0) and the radius r_0. At the same time, the coordinates of the four vertices CDEF of the rectangle are given as (c_1, d_1), (c_2, d_2), (c_3, d_3), and (c_4, d_4). Solve for the intersection relationship between the circle and the rectangle.

[0102] First, translate the center of the rectangle to the origin of the established rectangular coordinate system, keeping the relative positions of the rectangle and the circle fixed, while the circle translates accordingly; then rotate the rectangle so that one side is parallel to the fixed coordinate system, while keeping the relative positions fixed, the circle also rotates accordingly, as shown below. Figure 6 As shown. The center of the circle may be in any quadrant at this point. Taking the absolute values ​​of the center coordinates, the circle is now mirrored to the first quadrant. Assume the vector from the corner of the rectangle in the first quadrant to the center of the circle is... At this time, the vector There are four possible relative positions between the circle and the rectangle, and the expression for the minimum distance u between them also differs, according to... Figure 6 As shown, the minimum distance u is taken as different values ​​depending on the different situations:

[0103] In case (1), x>0 and y>0, at this time

[0104] In case (2), x < 0 and y > 0, then u = y.

[0105] In case (3), x>0 and y<0, u=x.

[0106] In case (4), x < 0 and y < 0, then u = 0.

[0107] In four cases, when the radius r > u, the rectangle and the circle do not intersect.

[0108] (3) Methods for determining the intersection of rectangles

[0109] like Figure 7As shown, the separation axis theorem is used to determine the collision relationships between polygons. The idea behind the separation axis theorem is that for two polygons, if there exists a straight line separating them, then the two polygons do not intersect. For the separation axis theorem between rectangles, it is only necessary to determine if there exists an axis parallel to the side of either rectangle that can separate the two rectangles; a maximum of four axes need to be considered. Taking two arbitrary rectangles HIJK and QRPT in space as an example, let the two rectangles be rectangle H and rectangle N. Given the vertex coordinates of the two rectangles as (f_1, g_1), (f_2, g_2), (f_3, g_3), (f_4, g_4) and (m_1, n_1), (m_2, n_2), (m_3, n_3), (m_4, n_4), solve for the collision relationships between the rectangles. The specific calculation steps are as follows.

[0110] First, translate the center of rectangle H to the origin, keeping the relative positions unchanged, and translate rectangle N accordingly. Then, rotate rectangle H around the origin until one side is parallel to the coordinate axes, again keeping the relative positions unchanged, and rotate rectangle N accordingly, resulting in the transformed coordinates of the two rectangles. At this point, rectangle H has two distinct x-coordinate values ​​at its four corners, and rectangle N has four distinct x-coordinate values. Take the interval formed by the maximum and minimum values ​​of rectangle N's four coordinate values ​​and determine if it overlaps with the interval formed by two coordinate values ​​of rectangle H. Continue this process to check the y-axis. At this point, the separation axis for cases where rectangle H's two sides are parallel has been determined. If the x-axis and y-axis coordinate intervals do not overlap, the two rectangles do not intersect. If they overlap, rectangle N needs to be moved to the origin, and the above operation repeated to complete the determination of parallelism across the four axes of the two rectangles.

[0111] Step 3: Based on the given robot pose and joint angles, determine the internal collision situation:

[0112] 3.1 Simplified method for internal collision

[0113] Both a single leg and the manipulator can be considered as a two-degree-of-freedom chain structure. Collisions can be avoided within a single chain by limiting the range of joint angles. That is, for the leg joints, the thigh, lower leg and base of the same leg will not collide with each other; for the manipulator, the upper arm and lower arm and base will not collide with each other.

[0114] By employing the concept of quadtree collision detection spatial segmentation, we can effectively eliminate components in the robot's space that will not collide with each other. For example, legs on the diagonal will not collide, while the manipulator will only collide with the two adjacent legs. The collision situation inside the robot is independent of the robot's position and rotation angle, and only depends on the robot's ten joints. Therefore, we can directly divide the robot into four quadrants based on its structure, using the center of the square corresponding to the robot base as the origin and the two mutually perpendicular axes of symmetry of the robot base as the x and y coordinate axes. Figure 8 As shown. Based on the forward kinematics method described in step 1, the absolute position of each component's projected shape in the coordinate system is calculated using given joint angles and the structural dimensions of each robot component. The quadrant of each geometric shape is determined by its absolute position coordinates. If a shape intersects a coordinate axis, it is placed in two adjacent quadrants for separate calculations. Since collisions between the legs and the manipulator and the base are excluded, collision detection is not performed on the base using a quadtree. By determining collisions within each quadrant, we can significantly reduce the computational load for collisions at greater spatial distances, thereby improving computational speed.

[0115] 3.2 Internal Collision Detection Algorithm

[0116] For each quadrant of the quadtree partitioning, the three judgment methods described in step 2 are used to determine intersection of the graphics corresponding to different legs or manipulators. If any graphics intersect within a quadrant, the collision result is immediately returned to avoid unnecessary calculations. If no collision occurs, the judgment of graphics within the next quadrant continues until all four quadrants have been judged. If no intersecting graphics exist, a no-collision result is returned. For example... Figure 8 In the case shown, quadrants 2 and 3 contain only legs on one branch, so no intersection judgment is needed. The operating arm is located at the intersection of quadrants 1 and 4. At this time, it is only necessary to judge the intersection relationship between the legs in quadrant 1 and the operating arm and the legs in quadrant 4 based on the judgment of the intersection of the figures in step 2.

[0117] Step 4: Simplify the external environment based on the robot's structure and collision characteristics:

[0118] The external environment mainly consists of a quarter-spherical shell, a semi-circular partition, and a tube sheet. The robot operates within the quarter-sphere enclosed by these three elements. The robot moves in a two-dimensional direction on the tube sheet, and collisions between the tube sheet and the robot do not need to be considered. When dealing with the external environment, the partition portion can be considered height-independent and can be converted into a straight line in the two-dimensional plane. Figure 9As shown, the spherical shell gradually decreases in size with increasing height. The collision between the robot's moving body and the spherical shell first occurs in the plane corresponding to the robot's highest point. Therefore, for a robot without a manipulator, the collision between a robot of height h and a spherical shell with radius r can be transformed into the collision between the robot's highest cross-section and its radius sqrt(r). 2 -h 2 The collision of a semicircular arc is called the body collision semicircle.

[0119] The approach to handling the collision between the robot's manipulator arm and the probe is similar. The heights of their collision cross-sections correspond to the heights of the top surface of the manipulator arm's main arm and the probe's head, respectively; the corresponding semicircles are called the manipulator arm collision semicircle and the probe collision semicircle, respectively. Since the robot's structure has approximately the same shape in the height direction, the robot's cross-section can be replaced by the set of two-dimensional geometric figures described in step 1. The three-dimensional model collision problem is transformed into determining whether the robot's projected envelope is completely within the semicircle of the corresponding height.

[0120] Step 5: Based on the simplified external environment results and the given robot pose and joint angles, determine the collision situation between the robot and the external environment:

[0121] 5.1 Preliminary External Testing Methods

[0122] In locations far from the spherical shell and partitions, precise collision detection, while necessary to ensure efficient inspection speed, may increase computation time and thus impact work efficiency. Therefore, we divide the robot's collision detection with the external environment into two stages: preliminary external detection and precise detection, to delineate safe and key inspection areas.

[0123] The first step is an initial external inspection phase to rule out relatively safe locations. If the initial external inspection indicates that the robot may collide with something, further, more precise inspections are then conducted.

[0124] The initial external inspection phase employed the OBB bounding box method. The robot's 2D projected image set included rectangles and circles, requiring feature point extraction to generate the OBB bounding box. The specific steps were as follows: for rectangles, four vertices were selected as feature points; for circles, the center was selected as the feature point. An initial OBB bounding box was generated using all feature points, as shown below. Figure 10 As shown in the rectangle with the black line in the middle, the bounding box is expanded outward by the radius of the leg circle to obtain the final OBB bounding box, as shown. Figure 10The rectangle shown in red. Although the bounding box obtained by this method may not be the smallest, it uses the fewest feature points. Next, it is checked whether the generated bounding box is within the smallest of the three collision semicircles (body collision semicircle, manipulator collision semicircle, and probe collision semicircle) described in step 4 (manipulator collision semicircle, manipulator collision semicircle, and probe collision semicircle). If the bounding box is contained within the manipulator collision semicircle, it means that the robot has no collision with the external environment; otherwise, further precise detection algorithms are needed to check the collision between the robot and the external environment.

[0125] 5.2 Precise External Detection Algorithm

[0126] To determine whether a rectangle projected onto any plane lies within any semicircle, the rectangle is considered to be within a semicircle if all four vertices are within it. In 2D geometry, the four vertices of the rectangles corresponding to the thigh and lower leg are contained within the leg joint circle. Therefore, it's unnecessary to separately determine the intersection relationship between the thigh and lower leg rectangles and the collision semicircle of the robot body. Simply checking if the leg circle is within the collision semicircle is sufficient to determine if the robot's leg projection is completely within it. This process is repeated for each leg circle.

[0127] In certain special circumstances, the base may collide with the outside world, so it is also necessary to determine whether the base is completely within the collision semicircle of the main body.

[0128] Finally, determine whether the upper arm circle is within the operating arm collision semicircle, and at the same time determine whether the forearm circle and the forearm end probe rectangle are within the probe collision semicircle.

[0129] Figure 12 Two-dimensional simulation of a set of experiments for the maintenance robot. Figure 13 This document presents a 3D simulation of an experiment involving a repair robot. Within the range of motion of each joint, 1000 different base poses and joint angle states of the robot were randomly generated. Internal and external collision detection experiments were conducted on a 2D simulation platform using the algorithm studied in this invention. Collision detection results and their execution times were recorded, and the same data was used to obtain actual collision results on a 3D simulation platform.

[0130]

[0131] Observing the simulation results, it was found that although the accuracy was high, there were still discrepancies between the calculations and the 3D simulation. Further investigation revealed that some discrepancies were caused by errors in robot movement and deformation due to structural stiffness, leading to inconsistencies between the robot's motion and the actual situation. Other discrepancies were due to the use of redundant rectangular simulations for the end effector of the manipulator compared to the actual model. For higher accuracy, simulations with less redundancy could be used, but this would result in longer computation time. These problems all occurred inside the robot or near its external boundaries. Since the robot studied in this patent requires extremely high safety, these dangerous locations should be discarded during actual movement. Therefore, the accuracy of this algorithm can achieve the intended function while ensuring efficient computation. Specific Implementation Method Two:

[0133] This embodiment is a rapid collision detection system for a heat transfer tube maintenance robot, including the following steps:

[0134] Equivalent geometric units for the robot: Based on the structure of the heat transfer pipe maintenance robot, each component of the robot is projected onto a horizontal plane in two dimensions and represented as an equivalent geometric shape: the base is equivalent to a rectangle; the rotational joint connecting the thigh to the base is equivalent to a circle; the equivalent geometric shape of the thigh includes: the thigh torso portion is equivalent to a rectangle, and the rotational joint between the thigh and the lower leg is equivalent to a circle; the equivalent geometric shape of the lower leg includes: the lower leg torso portion is equivalent to a rectangle, and the extension joint at the end of the lower leg is equivalent to a circle; the rotational joint connecting the upper arm to the base is equivalent to a circle; the equivalent geometric shape of the upper arm includes: the upper arm torso portion is equivalent to a rectangle, and the rotational joint connecting the forearm to the upper arm is equivalent to a circle; the equivalent geometric shape of the forearm includes: the forearm torso portion is equivalent to a rectangle, and the end of the forearm is equivalent to a circle; the probe at the end of the forearm is equivalent to a rectangle.

[0135] The heat transfer pipe maintenance robot structure includes a mobile body and an operating arm. The mobile body includes a base and multiple legs symmetrically arranged on the base. Each leg includes a thigh and a calf. The thigh is connected to the base via a first leg joint, and the calf is connected to the thigh via a second leg joint. The first and second leg joints are rotary joints. The operating arm includes a large arm and a forearm. The large arm is connected to the center of the base via a first arm joint, and the forearm is connected to the large arm via a second arm joint. The first and second arm joints are rotary joints. A probe is located at the end of the forearm.

[0136] An internal collision detection unit based on equivalent geometry is used: The robot is divided into four quadrants with the center of the geometry corresponding to the robot base as the origin and the two mutually perpendicular axes of symmetry of the robot base as the x and y coordinate axes. The absolute position of the geometric shape corresponding to each component in the two-dimensional projection set of the robot is calculated in this coordinate system. The quadrant of each geometric shape is determined by the absolute position coordinates, and collision calculation is performed in the corresponding quadrant. If a shape intersects with a coordinate axis, the corresponding shape is placed in two adjacent quadrants, and collision calculation is performed in each quadrant. The collision calculation determines whether the geometric shapes corresponding to different components intersect; if they intersect, it is a collision; otherwise, there is no collision.

[0137] The external collision detection unit based on equivalent geometry includes a collision semicircle analysis module, a preliminary external detection module, and a precise external detection module.

[0138] Collision semicircle analysis module: Determines the collision semicircles of the robot body, the manipulator arm, and the probe based on the robot's structure and external environment.

[0139] The external environment includes a quarter-spherical shell, a semi-circular partition, and a tube sheet; the robot works within the quarter-sphere enclosed by these three components.

[0140] For a moving body, the collision between a robot of height h and a spherical shell of radius r is transformed into the collision of the robot's highest cross section with radius sqrt(r). 2 -h 2 The collision of the semicircular arc of the robot is called the body collision semicircle; for the robot's manipulator arm and probe, the collision section heights of the manipulator arm and probe are respectively the heights of the top surface of the manipulator arm and the probe, and the corresponding semicircles are called the manipulator arm collision semicircle and the probe collision semicircle, respectively.

[0141] Preliminary external detection module: Generates the OBB bounding box of the robot's 2D projection graphic set, and checks whether the generated bounding box is within the manipulator collision semicircle; if the bounding box is contained within the manipulator collision semicircle, it means that the robot has no collision with the external environment.

[0142] The precise external detection module determines whether the leg circle in the equivalent geometry of the thigh and the leg circle in the equivalent geometry of the calf are within the body collision semicircle; if so, no collision is determined. It also determines whether the upper arm circle in the equivalent geometry of the upper arm is within the operating arm collision semicircle; if so, no collision is determined. Simultaneously, it determines whether the forearm circle in the equivalent geometry of the forearm and the rectangle corresponding to the forearm end probe are within the probe collision semicircle; if so, no collision is determined. It is also used to determine whether the rectangle equivalent to the base is within the body collision semicircle; if so, no collision is determined. Specific implementation method three:

[0144] This embodiment is a rapid collision detection device for a heat transfer pipe maintenance robot. The device includes a processor and a memory. It should be understood that it includes any device including a processor and a memory described in this invention. The device may also include other units and modules that perform display, interaction, processing, control and other functions through signals or instructions.

[0145] The memory stores at least one instruction, which is loaded and executed by the processor to form a rapid collision detection system for a heat transfer tube maintenance robot.

[0146] 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.

[0147] The above examples of the present invention are merely illustrative of the computational model and process of the present invention, and are not intended to limit the implementation of the present invention. Those skilled in the art will recognize that other variations or modifications can be made based on the above description. It is impossible to exhaustively list all possible implementations here. Any obvious variations or modifications derived from the technical solutions of the present invention are still within the scope of protection of the present invention.

Claims

1. A rapid collision detection method for a heat transfer pipe maintenance robot, wherein the heat transfer pipe maintenance robot structure includes a mobile body and an operating arm; the mobile body includes a base and multiple legs symmetrically arranged on the base; each leg includes a thigh and a calf, the thigh being connected to the base via a first leg joint, and the calf being connected to the thigh via a second leg joint, the first leg joint and the second leg joint being rotary joints; the operating arm includes an upper arm and a forearm, the upper arm being connected to the center of the base via a first arm joint, and the forearm being connected to the upper arm via a second arm joint, the first arm joint and the second arm joint being rotary joints; a probe is located at the end of the forearm; characterized in that... The method includes the following steps: First, based on the structure of the heat transfer pipe maintenance robot, each component of the robot is projected onto a horizontal plane in two dimensions and represented as an equivalent geometric shape: the base is equivalent to a rectangle; the rotational joint connecting the thigh to the base is equivalent to a circle, and the equivalent geometric shape of the thigh includes: the thigh torso portion is equivalent to a rectangle, and the rotational joint between the thigh and the lower leg is equivalent to a circle; the equivalent geometric shape of the lower leg includes: the lower leg torso portion is equivalent to a rectangle, and the extension joint at the end of the lower leg is equivalent to a circle; the rotational joint connecting the upper arm to the base is equivalent to a circle, and the equivalent geometric shape of the upper arm includes: the upper arm torso portion is equivalent to a rectangle, and the rotational joint connecting the forearm to the upper arm is equivalent to a circle; the equivalent geometric shape of the forearm includes: the forearm torso portion is equivalent to a rectangle, and the end of the forearm is equivalent to a circle; the probe at the end of the forearm is equivalent to a rectangle. Then, internal collision detection and external collision detection are performed based on equivalent geometry; The internal collision detection based on equivalent geometry includes: using the center of the geometry corresponding to the robot base as the origin and the two mutually perpendicular axes of symmetry of the robot base as the x and y coordinate axes, directly dividing the robot into four quadrants, and calculating the absolute position of the geometric shape corresponding to the projection of each component in the two-dimensional projection geometry set of the robot in this coordinate system. The quadrant of each geometric shape is determined by the absolute position coordinates, and the geometry collision calculation is performed in the corresponding quadrant. If the shape intersects with the coordinate axis, the corresponding shape is placed in two adjacent quadrants, and the geometry collision calculation is performed in each quadrant. The geometry collision calculation determines whether the geometric shapes corresponding to different components intersect; if they intersect, it is a collision; otherwise, it is not a collision. The external collision detection based on equivalent geometry includes: The external environment consists of a quarter-spherical shell, a semi-circular partition, and a tube sheet; the robot works within the quarter-sphere enclosed by these three elements. For a moving body, the collision between a robot of height h and a spherical shell of radius r is transformed into the collision of the robot's highest cross section with radius sqrt(r). 2 -h 2 The collision of a semicircular arc of the body is called the body collision semicircle. For the robot's manipulator arm and probe, the collision section heights of the manipulator arm and probe correspond to the heights of the top surface of the manipulator arm and the probe, respectively, and the corresponding semicircles are called the manipulator arm collision semicircle and the probe collision semicircle, respectively. First, perform preliminary external detection: generate an OBB bounding box for the robot's 2D projection graphic set, and check whether the generated bounding box is within the manipulator collision semicircle; if the bounding box is contained within the manipulator collision semicircle, it means that the robot has no collision with the external environment; otherwise, perform precise external detection. Precise external detection: Determine whether the leg circle in the equivalent geometry of the thigh and the leg circle in the equivalent geometry of the calf are within the body collision semicircle. If they are, no collision is determined. Determine whether the upper arm circle in the equivalent geometry of the upper arm is within the operating arm collision semicircle. If they are, no collision is determined. At the same time, determine whether the forearm circle in the equivalent geometry of the forearm and the rectangle corresponding to the probe at the end of the forearm are within the probe collision semicircle. If they are, no collision is determined.

2. The rapid collision detection method for a heat transfer pipe maintenance robot according to claim 1, characterized in that, Before performing internal collision detection, each leg and the manipulator can be regarded as a two-degree-of-freedom chain structure. Collisions are avoided within the single chain corresponding to the manipulator or each leg by limiting the range of joint angles. When performing internal collision detection based on equivalent geometry, collision detection of the corresponding geometry within the single chain of the manipulator or each leg is excluded.

3. The rapid collision detection method for a heat transfer tube maintenance robot according to claim 1, characterized in that, When starting internal collision detection based on equivalent geometry, the quadtree collision detection space segmentation method is first used to exclude parts in the robot space that will not collide with each other, and then internal collision detection is performed based on equivalent geometry.

4. A rapid collision detection method for a heat transfer pipe maintenance robot according to claim 1, 2, or 3, characterized in that, In the process of calculating the absolute position of the geometric figure corresponding to the projection of each component in the two-dimensional projection graphic set of the robot in the coordinate system, based on the positive kinematic equations of the robot's legs and manipulator, and through the given joint angles and structural dimensions of each component of the robot, the absolute position of the projection of each component in the two-dimensional projection graphic set of the robot in the coordinate system is calculated. The forward kinematic equation of the leg is as follows: Where i represents the i-th leg; θ i1 Let θ be the rotation angle of the first leg joint of leg i. i2 Let be the rotation angle of the second leg joint of leg i; L1 is the thigh length, L2 is the calf length; d2 is the distance between the first and second leg joints on each leg on the Z-axis in the world coordinate system; d4 is the distance between the telescoping joint and the foot end on the Z-axis in the world coordinate system; d i3 Let be the distance the extension joint of leg i moves.

5. The rapid collision detection method for a heat transfer pipe maintenance robot according to claim 4, characterized in that, The process of determining whether the geometric figures corresponding to different components intersect includes: (1) Determining the intersection of two circles: For two circles, denoted as circle A and circle B, which are equivalent to circles in a two-dimensional projected graphic space, let their centers be (x_1, y_1) and (x_2, y_2) respectively, and their radii be r_1 and r_2 respectively; the intersection problem is transformed into the following equation: (2) Determining the intersection of a circle and a rectangle: Given an equivalent circle and a rectangle in a two-dimensional projected graphic space, denoted as circle M and rectangle CDEF, with the center M of the circle having coordinates (x_0, y_0) and radius r_0, and the four vertices CDEF of the rectangle having coordinates (c_1, d_1), (c_2, d_2), (c_3, d_3), and (c_4, d_4), solve for the intersection relationship between the circle and the rectangle. First, translate the center of the rectangle to the origin of the established rectangular coordinate system, keeping the relative positions of the rectangle and the circle fixed, and the circle translates accordingly. Then, rotate the rectangle so that one side is parallel to the fixed coordinate system, keeping the relative positions fixed, and the circle rotates accordingly as well. At this point, the center of the circle may be in any quadrant. Take the absolute values ​​of the center coordinates; the circle is now mirrored to the first quadrant. Assume the vector from the corner of the rectangle in the first quadrant to the center of the circle... At this time, the vector There are four possibilities: In case (1), x>0 and y>0, at this time In case (2), x < 0 and y > 0, then u = y; In case (3), x>0 and y<0, then u=x; In case (4), x < 0 and y < 0, then u = 0; In four cases, when the radius r > u, the rectangle and the circle do not intersect. (3) Determining the intersection of rectangles: For two equivalent rectangles in a two-dimensional projected graphic space, denoted as rectangle HIJK and QRPT, let the two rectangles be rectangle H and rectangle N respectively; given the vertex coordinates of the two rectangles as (f_1,g_1), (f_2,g_2), (f_3,g_3), (f_4,g_4) and (m_1,n_1), (m_2,n_2), (m_3,n_3), (m_4,n_4), solve the collision relationship of the rectangles. The specific calculation steps are as follows; First, translate the center of rectangle H to the origin, keeping the relative position unchanged, and also translate rectangle N accordingly. Then, rotate rectangle H around the origin until one side is parallel to the coordinate axes, keeping the relative position unchanged, and also rotate rectangle N accordingly, resulting in the transformed coordinates of the two rectangles. At this point, rectangle H has two different x-coordinate values ​​at its four corners, and rectangle N has four different x-coordinate values. Take the interval formed by the maximum and minimum values ​​of the four coordinate values ​​of rectangle N and determine if it overlaps with the interval formed by two coordinate values ​​of rectangle H. Continue this process to determine the y-axis. At this point, the determination of whether there is a separating axis when the rectangle is parallel to two sides of rectangle H has been completed. If there is a non-overlapping coordinate interval between the x and y axes, the two rectangles do not intersect. If there is an overlap, rectangle N needs to be moved to the origin, and the above operation repeated to complete the determination of parallelism to the four axes of the two rectangles.

6. The rapid collision detection method for a heat transfer tube maintenance robot according to claim 1, characterized in that, The process of generating the OBB bounding box of the robot's 2D projected graphic set includes: The robot's 2D projection graphic set includes rectangles and circles. Feature points of all geometric shapes are extracted. For rectangles, four vertices are selected as feature points. For circles, the center is selected as the feature point. An initial OBB bounding box is generated using all feature points. This bounding box is then expanded outward by the radius of the leg circle to obtain the final OBB bounding box.

7. The rapid collision detection method for a heat transfer pipe maintenance robot according to claim 1, characterized in that, During the precise external detection process, it is also determined whether the rectangle equivalent to the base is within the collision semicircle of the main body. If it is within the collision semicircle of the main body, it is determined that there is no collision.

8. A rapid collision detection system for a heat transfer tube maintenance robot, characterized in that, include: Equivalent geometric units for the robot: Based on the structure of the heat transfer pipe maintenance robot, each component of the robot is projected onto a horizontal plane in two dimensions and represented as an equivalent geometric shape: the base is equivalent to a rectangle; the rotational joint connecting the thigh to the base is equivalent to a circle; the equivalent geometric shape of the thigh includes: the thigh torso portion is equivalent to a rectangle, and the rotational joint between the thigh and the lower leg is equivalent to a circle; the equivalent geometric shape of the lower leg includes: the lower leg torso portion is equivalent to a rectangle, and the extension joint at the end of the lower leg is equivalent to a circle; the rotational joint connecting the upper arm to the base is equivalent to a circle; the equivalent geometric shape of the upper arm includes: the upper arm torso portion is equivalent to a rectangle, and the rotational joint connecting the forearm to the upper arm is equivalent to a circle; the equivalent geometric shape of the forearm includes: the forearm torso portion is equivalent to a rectangle, and the end of the forearm is equivalent to a circle; the probe at the end of the forearm is equivalent to a rectangle. The heat transfer pipe maintenance robot structure includes a mobile body and an operating arm. The mobile body includes a base and multiple legs symmetrically arranged on the base. Each leg includes a thigh and a calf. The thigh is connected to the base via a first leg joint, and the calf is connected to the thigh via a second leg joint. The first and second leg joints are rotary joints. The operating arm includes a large arm and a forearm. The large arm is connected to the center of the base via a first arm joint, and the forearm is connected to the large arm via a second arm joint. The first and second arm joints are rotary joints. A probe is located at the end of the forearm. An internal collision detection unit based on equivalent geometry is used: The robot is divided into four quadrants with the center of the geometry corresponding to the robot base as the origin and the two mutually perpendicular axes of symmetry of the robot base as the x and y coordinate axes. The absolute position of the geometric shape corresponding to each component in the two-dimensional projection set of the robot is calculated in this coordinate system. The quadrant of each geometric shape is determined by the absolute position coordinates, and collision calculation is performed in the corresponding quadrant. If a shape intersects with a coordinate axis, the corresponding shape is placed in two adjacent quadrants, and collision calculation is performed in each quadrant. The collision calculation determines whether the geometric shapes corresponding to different components intersect; if they intersect, it is a collision; otherwise, there is no collision. The external collision detection unit based on equivalent geometry includes a collision semicircle analysis module, a preliminary external detection module, and a precise external detection module. Collision semicircle analysis module: Determines the collision semicircles of the robot body, the manipulator arm, and the probe based on the robot's structure and external environment. The external environment includes a quarter-spherical shell, a semi-circular partition, and a tube sheet; the robot works within the quarter-sphere enclosed by these three components. For a moving body, the collision between a robot of height h and a spherical shell of radius r is transformed into the collision of the robot's highest cross section with radius sqrt(r). 2 -h 2 The collision of the semicircular arc of the robot is called the body collision semicircle; for the robot's manipulator arm and probe, the collision section heights of the manipulator arm and probe are respectively the heights of the top surface of the manipulator arm and the probe, and the corresponding semicircles are called the manipulator arm collision semicircle and the probe collision semicircle, respectively. Preliminary external detection module: Generates the OBB bounding box of the robot's 2D projection graphic set, and checks whether the generated bounding box is within the manipulator collision semicircle; if the bounding box is contained within the manipulator collision semicircle, it means that the robot has no collision with the external environment. Precise external detection module: Determines whether the leg circle in the equivalent geometry of the thigh and the leg circle in the equivalent geometry of the calf are within the body collision semicircle. If they are, no collision is determined. Determines whether the upper arm circle in the equivalent geometry of the upper arm is within the operating arm collision semicircle. If they are, no collision is determined. Simultaneously, it determines whether the forearm circle in the equivalent geometry of the forearm and the rectangle corresponding to the probe at the end of the forearm are within the probe collision semicircle. If they are, no collision is determined.

9. The rapid collision detection system for a heat transfer tube maintenance robot according to claim 8, characterized in that, The precise external detection module is also used to determine whether the rectangle equivalent to the base is inside the collision semicircle of the body, and if it is inside the collision semicircle of the body, it is determined that there is no collision.

10. A rapid collision detection device for a heat transfer tube maintenance robot, 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 as described in any one of claims 8 to 9.

Images