Thermal power station-oriented multi-automatic engineering equipment cooperative positioning and mapping method and thermal power station-oriented multi-automatic engineering equipment cooperative positioning and mapping device

Abstract

The invention discloses a thermal power station-oriented multi-automatic engineering equipment cooperative positioning and mapping method and device, and the method comprises the steps: numbering two pieces of cabin cleaning engineering equipment in a target region as TP0 and TP1, collecting the point cloud data of the surrounding environment by each piece of cabin cleaning engineering equipment, and enabling the point cloud data to comprise a plurality of frames of point clouds; according to the method and the device, the positioning and the pose of the opposite cabin cleaning engineering equipment are calculated through each frame of point cloud collected by each cabin cleaning engineering equipment, then the positioning of each cabin cleaning engineering equipment is obtained, the point cloud map of each cabin cleaning engineering equipment is calculated for each frame of point cloud, mapping of each cabin cleaning engineering equipment is realized, and the positioning accuracy of the cabin cleaning engineering equipment is improved. The operation efficiency of cabin cleaning engineering equipment and a wharf is improved; according to the scheme, data transmission among the cabin cleaning engineering equipment can be reduced through the point cloud data, the data pressure in the aspect of communication is reduced, the cabin cleaning engineering equipment maps with large numbers are converted and combined with the cabin cleaning engineering equipment maps with small numbers, and a complete collaborative mapping scheme of the multiple cabin cleaning engineering equipment is formed.

Description

Technical Field

[0001] This invention belongs to the field of intelligent vehicle perception technology, specifically relating to a method and device for collaborative positioning and mapping of multiple automated engineering equipment in thermal power plants. Background Technology

[0002] The coal unloading and cleaning operation at thermal power plant terminals mainly involves lowering cleaning vehicles such as pushers and excavators into the ship's hold. Through pushing, scraping, and digging operations, the coal in the hold is gathered to the area accessible to the unloader, which then works in conjunction with the unloader located above the hold to grab the coal pile. However, due to the mechanical limitations of the unloader's grab bucket, it cannot reach the edges of the hold, such as the bulkheads and corners, resulting in a large amount of residual coal remaining after the operation. To recover this residual coal, cleaning equipment is needed to concentrate and pile it in the center of the hold to the area accessible to the unloader, where it is then grabbed and transported out. Therefore, the cleaning equipment is a key piece of equipment connecting the unloader and completing the coal unloading process.

[0003] Currently, the cleaning operation of ship hold cleaning equipment in the industry is still mainly manual: operators need to drive the cleaning equipment into the ship hold, visually observe the environment, and manually control the equipment's movement, coal shoveling, and stacking actions. This traditional operation mode has safety hazards. The unloading machine may fall and land on the cleaning equipment, causing injury. Furthermore, the narrow working space and poor air circulation inside the ship hold result in coal dust concentrations far exceeding occupational health standards, which can cause irreversible damage to the operator's respiratory system with prolonged exposure. Additionally, under current conditions, when only one cleaning machine is operating in a single ship hold, the operation efficiency is low, the single cleaning cycle is long, and the accuracy of the coal stacking position is insufficient, with significant deviations. This forces the unloading machine to frequently adjust its position, further reducing overall unloading efficiency.

[0004] Existing patent CN120063283A describes a multi-machine real-time collaborative positioning method and device for indoor scenes, providing a method for multi-machine collaborative positioning indoors. However, the local semantic map used in this solution requires a large amount of data and needs to be in an environment with good geometric features, such as indoors. Existing patent CN120760704A describes a method, device, and storage medium based on multi-robot collaborative positioning, providing a solution for multi-machine collaborative positioning and map storage. However, this method uses relatively simple sensors, and the data exchanged between robots is large, making it unsuitable for high-temperature and high-dust environments such as ship cabins. Summary of the Invention

[0005] The purpose of this invention is to address the problems raised in the background art by proposing a collaborative positioning and mapping method and device for multiple automated engineering equipment in thermal power plants.

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

[0007] This invention proposes a collaborative positioning and mapping method for multiple automated engineering equipment in thermal power plants, comprising:

[0008] Step 1: Number the two cleaning equipment in the target area as TP0 and TP1. Each cleaning equipment collects point cloud data of the surrounding environment. The point cloud data contains multiple frames of point cloud. Each frame of point cloud is preprocessed. Each preprocessed frame of point cloud contains n first three-dimensional points on the other cleaning equipment and N second three-dimensional points other than the other cleaning equipment.

[0009] For each point cloud frame collected by the various cabin clearing engineering equipment, follow these steps:

[0010] Step 2: Each cleaning equipment calculates the positioning and pose of the enemy's cleaning equipment using the n first three-dimensional points collected.

[0011] Step 3: Convert the N second three-dimensional points into a two-dimensional matrix;

[0012] Step 4: Each cleaning equipment transmits the two-dimensional matrix corresponding to the N second three-dimensional points it collected to the other cleaning equipment. Each cleaning equipment only receives the two-dimensional matrix transmitted by the cleaning equipment with a smaller number than itself. That is, cleaning equipment TP1 receives the two-dimensional matrix transmitted by cleaning equipment TP0 and transforms it into its own coordinate system.

[0013] Step 5: The TP1 cleaning equipment calculates the first rotation matrix and the first translation vector of the two-dimensional matrix corresponding to the current frame point cloud based on the two-dimensional matrix corresponding to the current frame point cloud, the received two-dimensional matrix, and the two-dimensional matrix corresponding to the historical frame point cloud, and calculates the second rotation matrix and the second translation vector of the received two-dimensional matrix.

[0014] Step 6: The cleaning equipment TP1 merges the N second three-dimensional points corresponding to the two-dimensional matrix transformed into its own coordinate system with the N second three-dimensional points corresponding to all historical frame point clouds of the cleaning equipment TP1 to form the point cloud map of the cleaning equipment TP1.

[0015] Step 7: The cleaning equipment TP1 uses the first rotation matrix and the first translation vector to transform the N second three-dimensional points corresponding to the current frame point cloud to obtain the first transformed point cloud map. The first transformed point cloud map is merged into the point cloud map of the cleaning equipment TP1 to obtain the first point cloud map. Then, the first point cloud map is transformed using the second rotation matrix and the second translation vector to obtain the second transformed point cloud map. The cleaning equipment TP1 sends the second transformed point cloud map to the cleaning equipment TP0. The cleaning equipment TP0 merges the received second transformed point cloud map with the N second three-dimensional points corresponding to all its historical frame point clouds to form the point cloud map of the cleaning equipment TP0.

[0016] Preferably, the preprocessing includes: filtering where the horizontal distance between the sampled tank cleaning equipment and the target equipment exceeds a first preset range. The three-dimensional points, and the filtering height exceeding the second preset range. The three-dimensional points are defined as follows: the horizontal distance is the distance between the three-dimensional points on the horizontal and vertical coordinates and the collected cleaning equipment; the height is the coordinate of the three-dimensional points on the vertical coordinate; and the coordinates of each cleaning equipment in its respective coordinate system are (0,0,0).

[0017] Preferably, the step of calculating the positioning and pose of the enemy's clearing equipment using n first three-dimensional points includes:

[0018] The n first three-dimensional points of the current frame point cloud collected by the cleaning equipment TP0 or TP1 are represented as follows: The first positioning point of either the cleaning equipment TP1 or TP0 is... ;

[0019] Furthermore, calculate the second positioning points of either the cleaning equipment TP0 or TP1 corresponding to the n first three-dimensional points of the previous frame point cloud acquired by the cleaning equipment TP0 or TP1. Furthermore, each positioning point of the cleaning equipment TP1 or TP0 is represented in the coordinate system of the cleaning equipment TP0 or TP1.

[0020] Subtract the first positioning point from the second positioning point to obtain the displacement vector in the coordinate system of the cleaning equipment TP0 or TP1. ;

[0021] Let the unit vector of the X-axis in the coordinate system of the cleaning equipment TP0 or TP1 be... After the fifth rotation matrix The unit vector corresponding to the displacement vector is:

[0022] (1);

[0023] The unit vector corresponding to the displacement vector is also represented as:

[0024] (2);

[0025] in, The magnitude of the displacement vector, the fifth rotation matrix for , , and These are the heading angle, pitch angle, and roll angle, respectively. Let Z be the rotation matrix. Let Y be the rotation matrix around Y. Let X be the rotation matrix about X;

[0026] in,

[0027] ;

[0028] ;

[0029] ;

[0030] By combining formulas (1) and (2), the pose of either the cleaning equipment TP1 or TP0 can be obtained. .

[0031] Preferably, the step of converting N second three-dimensional points into a two-dimensional matrix includes:

[0032] N second-dimensional points are represented as follows: Converted to dimensions Two-dimensional matrix That is, a two-dimensional matrix The height is Width is and two-dimensional matrix for A grid, each cell being sized as follows: ,in The height of the grid. Let be the width of the grid, and , ;

[0033] Two-dimensional matrix Each element in is ,and Indicates the first Line number The maximum height among all extreme points within a column's grid. If there are no extreme points within the grid, then ;

[0034] in, , ,like The value exceeds Scope, or The value exceeds If the range is defined, then the corresponding extreme points will be filtered out. , That is, the second three-dimensional point The corresponding extreme points, , is the second three-dimensional point The horizontal distance from the corresponding cleanup equipment collected. For the second three-dimensional point The relative azimuth angle of the collected cabin cleaning equipment. This is the floor symbol.

[0035] Preferably, the process by which the cleaning equipment TP1 transforms the two-dimensional matrix received from the cleaning equipment TP0 into its own coordinate system includes:

[0036] The TP1 cleaning equipment restores the received two-dimensional matrix into N second three-dimensional points:

[0037] For each non-zero element in the two-dimensional matrix Restored to the second three-dimensional point First, set each element Convert to extreme point and , Then, based on the extreme points Calculate the second three-dimensional point ,and , , ;

[0038] like If the element is not specified, then skip that element.

[0039] Calculate the sixth rotation matrix and the sixth translation vector for transforming the TP0 coordinate system of the cleaning equipment into the TP1 coordinate system of the cleaning equipment, and use the sixth rotation matrix and the sixth translation vector to calculate the transformation matrix for transforming the TP0 coordinate system of the cleaning equipment into the TP1 coordinate system of the cleaning equipment.

[0040] The restored N second three-dimensional points are transformed using a transformation matrix, so that the cleaning equipment TP1 can transform the two-dimensional matrix received from the cleaning equipment TP0 into its own coordinate system.

[0041] Preferably, the cabin clearing equipment TP1 calculates the first rotation matrix and the first translation vector of the two-dimensional matrix corresponding to the current frame point cloud based on the two-dimensional matrix corresponding to the current frame point cloud, the received two-dimensional matrix, and the two-dimensional matrix corresponding to the historical frame point cloud, and calculates the second rotation matrix and the second translation vector of the received two-dimensional matrix, including:

[0042] Step 5.1: The two-dimensional matrix corresponding to the current frame point cloud collected by the TP1 cleaning equipment is called the first two-dimensional matrix. The first two-dimensional matrix is ​​compared with the two-dimensional matrices corresponding to all historical frame point clouds collected by the TP1 cleaning equipment. The two-dimensional matrices corresponding to the K historical frame point clouds with the highest similarity are selected and called the K first candidate loopback frames.

[0043] Step 5.2: Select the K first candidate loop closure frames in descending order of similarity;

[0044] Step 5.3: For the N second three-dimensional points corresponding to the first two-dimensional matrix Each second three-dimensional point in From the N second three-dimensional points corresponding to the selected first candidate loopback frame In the expression, find the nearest neighbor and represent it as... Then we get N nearest neighbors. ;

[0045] Step 5.4: Construct the first covariance matrix as follows: ;

[0046] in, , , , ;

[0047] in, The first two-dimensional matrix corresponds to the N points in the second three-dimensional matrix. The centering vector of the second three-dimensional point The first candidate loopback frame selected so far corresponds to the N second three-dimensional points. The centering vector of the second three-dimensional point The center point among the N second three-dimensional points corresponding to the first two-dimensional matrix. The center point is one of the N second three-dimensional points corresponding to the currently selected first candidate loopback frame;

[0048] Step 5.5: Apply singular value decomposition to the first covariance matrix. Decomposed into The third rotation matrix is ​​obtained. and the third translation vector ;in, The first covariance matrix The left singular vector matrix, The first covariance matrix diagonal matrix, The first covariance matrix The right singular vector matrix;

[0049] Step 5.6: Calculate the first error using the N nearest neighbors, the third rotation matrix, and the third translation vector. Then, it is determined whether the first error is greater than or equal to the first threshold. If it is greater than or equal to the first threshold, the next first candidate loopback frame is selected, and steps 5.3-5.6 are repeated. Otherwise, proceed to step 5.7, and the formula for the first error is as follows:

[0050] ;

[0051] Step 5.7: Calculate the second error using the N nearest neighbors, the third rotation matrix, and the third translation vector. Then, it is determined whether the second error is greater than or equal to the second threshold. If it is greater than or equal to the second threshold, the next first candidate loopback frame is selected, and steps 5.3-5.7 are repeated. Otherwise, the current first candidate loopback frame is taken as the real first loopback frame, and the formula for the second error is as follows:

[0052] ;

[0053] Step 5.8: Obtain the third rotation matrix and third translation vector corresponding to the actual first loop frame, and use them as the first rotation matrix and first translation vector;

[0054] Similarly, for the two-dimensional matrix received by the cleaning equipment TP1 from the cleaning equipment TP0, the same operation is performed according to steps 5.1-5.8 to obtain the second rotation matrix and the second translation vector.

[0055] A collaborative positioning and mapping device for multiple automated engineering equipment in a thermal power plant includes a processor and a memory storing a number of computer instructions. When the computer instructions are executed by the processor, they implement the steps of a collaborative positioning and mapping method for multiple automated engineering equipment in a thermal power plant.

[0056] Compared with the prior art, the beneficial effects of the present invention are as follows:

[0057] This method and device for collaborative positioning and mapping of multiple automated engineering equipment in thermal power plants calculates the positioning and pose of each cleaning engineering equipment by analyzing each frame of point cloud data collected from the equipment. This yields the positioning of each piece of equipment, and a point cloud map is calculated for each frame, thus achieving mapping of all cleaning equipment and improving the operational efficiency of both the equipment and the dock. This solution reduces data transmission between cleaning equipment by using point cloud data, thus reducing communication data pressure. Maps of equipment with higher numbers are converted and merged with those of equipment with lower numbers to form a complete collaborative mapping solution for multiple cleaning equipment. Attached Figure Description

[0058] Figure 1 This is a flowchart illustrating the collaborative positioning and mapping method for multiple automated engineering equipment in thermal power plants according to the present invention.

[0059] Figure 2 This is a schematic diagram illustrating the conversion of N second three-dimensional points into a two-dimensional matrix according to the present invention;

[0060] Figure 3 An example of a top view of the point cloud map of the TP0 cleaning equipment of the present invention;

[0061] Figure 4 This is an example of a side view of the point cloud map of the TP0 cleaning equipment of the present invention. Detailed Implementation

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

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

[0064] In one embodiment, such as Figures 1-4 As shown, a collaborative positioning and mapping method for multiple automated engineering equipment in thermal power plants is provided, including:

[0065] First, it should be noted that this method can be applied to the positioning and mapping of multiple cleaning equipment (two or more) in a target area. This solution takes two cleaning equipment as an example (execute steps 1-7) to achieve the positioning and mapping of the two cleaning equipment in a collaborative operation. Any two cleaning equipment in the target area can be operated according to steps 1-7 to achieve the positioning and mapping of the collaborative operation, thereby achieving the positioning and mapping of multiple cleaning equipment (two or more) in the target area in a collaborative operation.

[0066] Step 1: Number the two cleaning equipment in the target area as TP0 and TP1. Each cleaning equipment collects point cloud data of the surrounding environment (collected by lidar). The point cloud data contains multiple frames of point cloud. Each frame of point cloud is preprocessed, and each preprocessed frame of point cloud contains n first three-dimensional points on the other cleaning equipment, and N second three-dimensional points other than the other cleaning equipment.

[0067] The preprocessing process involves filtering the data and determining if the horizontal distance between the data and the corresponding collected data from the tank cleaning equipment exceeds a first preset range. The three-dimensional points, and the filtering height exceeding the second preset range. The three-dimensional points are defined as follows: the horizontal distance is the distance between the three-dimensional point's coordinates on the horizontal and vertical axes and the collected cleaning equipment; the height is the three-dimensional point's coordinates on the vertical axis. This refers to the point cloud data used for cleaning equipment TP0 (or TP1). In each frame of this point cloud data, the horizontal distance between the point cloud and cleaning equipment TP0 (or TP1) exceeds a first preset range. The 3D points are filtered, and points in each frame of the point cloud data whose height exceeds a second preset range are also filtered. The three-dimensional points are filtered, and the coordinates of the cleaning equipment TP0 (or TP1) in the coordinate system of the cleaning equipment TP0 (or TP1) are (0,0,0). For example, a three-dimensional point is represented as... Then the horizontal distance is The height is .

[0068] For each point cloud frame collected by each cleaning equipment, follow these steps (i.e., perform steps 2-7 for each point cloud frame collected by each cleaning equipment):

[0069] Step 2: Each cleaning equipment calculates the location and pose of the opposing cleaning equipment using the n first 3D points (from each frame of point cloud) acquired (i.e., cleaning equipment TP0 calculates the location and pose of cleaning equipment TP1 using the n first 3D points acquired, and cleaning equipment TP1 calculates the location and pose of cleaning equipment TP0 using the n first 3D points acquired), including:

[0070] The n first three-dimensional points of the current frame point cloud collected by the cleaning equipment TP0 (or cleaning equipment TP1) are represented as follows: , For the n first 3D points of the point cloud in the current frame The coordinates of the first three-dimensional point are given, and the first positioning point of the cleaning equipment TP1 (or cleaning equipment TP0) is: ;

[0071] Furthermore, calculate the second positioning points of the cleaning equipment TP1 (or TP0) corresponding to the n first three-dimensional points of the previous frame point cloud (the point cloud before the current frame point cloud, but this step is not performed for the first frame point cloud) collected by the cleaning equipment TP0 (or TP1). Furthermore, each positioning point of the cleaning equipment TP1 (or cleaning equipment TP0) is represented in the coordinate system of the cleaning equipment TP0 (or cleaning equipment TP1).

[0072] The displacement vector in the coordinate system of the cleaning equipment TP0 (or TP1) is obtained by subtracting the first positioning point from the second positioning point. ;

[0073] Let the unit vector of the X-axis in the coordinate system of the cleaning equipment TP0 (or cleaning equipment TP1) be... After the fifth rotation matrix The unit vector corresponding to the displacement vector is:

[0074] (1);

[0075] The unit vector corresponding to the displacement vector is also represented as:

[0076] (2);

[0077] in, The magnitude of the displacement vector, the fifth rotation matrix for , , and These are the heading angle, pitch angle, and roll angle, respectively. Let Z be the rotation matrix. Let Y be the rotation matrix around Y. Let X be the rotation matrix about X;

[0078] in,

[0079] ;

[0080] ;

[0081] ;

[0082] By combining formulas (1) and (2), the pose of the cleaning equipment TP1 (or cleaning equipment TP0) is obtained. .

[0083] Step 3, as follows Figure 2 As shown, the N second three-dimensional points are converted into a two-dimensional matrix (the N second three-dimensional points collected by the cleaning equipment TP1 and TP0 are all converted into a two-dimensional matrix), where Figure 2 Figure (a) shows the second three-dimensional point. Figure 2 Figure (b) shows the transformed two-dimensional matrix, including:

[0084] N second-dimensional points are represented as follows: , For N second three-dimensional points, the first The second three-dimensional point is converted to a dimension of Two-dimensional matrix That is, a two-dimensional matrix The height is Width is and two-dimensional matrix for A grid, each cell being sized as follows: ,in The height of the grid. Let be the width of the grid, and , ;

[0085] Two-dimensional matrix Each element in is ,and Indicates the first Line number The maximum height among all extreme points within a column's grid. If there are no extreme points within the grid, then ;

[0086] in, , ,like The value exceeds Scope, or The value exceeds If the range is defined, then the corresponding extreme points will be filtered out. , That is, the second three-dimensional point The corresponding extreme points, , is the second three-dimensional point The horizontal distance from the corresponding cleanup equipment collected. For the second three-dimensional point The relative azimuth angle of the collected cabin cleaning equipment. This is the floor symbol.

[0087] Step 4: Each cleaning equipment transmits the two-dimensional matrix corresponding to the N second three-dimensional points it collected to the other cleaning equipment. Each cleaning equipment only receives the two-dimensional matrix transmitted by the cleaning equipment with a smaller number than itself. That is, cleaning equipment TP1 receives the two-dimensional matrix transmitted by cleaning equipment TP0 and transforms it into its own coordinate system.

[0088] The process by which cleaning equipment TP1 transforms the two-dimensional matrix received from cleaning equipment TP0 into its own coordinate system includes:

[0089] The TP1 cleaning equipment restores the received two-dimensional matrix into N second three-dimensional points:

[0090] For each non-zero element in the two-dimensional matrix Restored to the second three-dimensional point First, set each element Convert to extreme point and , Then, based on the extreme points Calculate the second three-dimensional point ,and , , ;

[0091] like If the element is not specified, then skip that element.

[0092] The sixth rotation matrix and sixth translation vector are calculated to transform the TP0 coordinate system of the cleaning equipment into the TP1 coordinate system (by obtaining the coordinates of any point in the TP0 coordinate system and then obtaining the sixth rotation matrix and sixth translation vector, this process belongs to the prior art), and the transformation matrix for transforming the TP0 coordinate system of the cleaning equipment into the TP1 coordinate system is calculated using the sixth rotation matrix and sixth translation vector (this process also belongs to the prior art).

[0093] The restored N second three-dimensional points are transformed using a transformation matrix, so that the cleaning equipment TP1 can transform the two-dimensional matrix received from the cleaning equipment TP0 into its own coordinate system.

[0094] Step 5: The TP1 cleaning equipment calculates the first rotation matrix and the first translation vector of the two-dimensional matrix corresponding to the current frame point cloud, based on the two-dimensional matrix corresponding to the current frame point cloud, the received two-dimensional matrix, and the two-dimensional matrix corresponding to the historical frame point cloud. It also calculates the second rotation matrix and the second translation vector of the received two-dimensional matrix, including:

[0095] Step 5.1: The two-dimensional matrix corresponding to the current frame point cloud collected by the TP1 cleaning equipment is called the first two-dimensional matrix. The first two-dimensional matrix is ​​compared with the two-dimensional matrices corresponding to all historical frame point clouds collected by the TP1 cleaning equipment (e.g., similarity calculation using cosine distance). The two-dimensional matrices corresponding to the K historical frame point clouds with the highest similarity are selected and called the K first candidate loopback frames.

[0096] Step 5.2: Select the K first candidate loop closure frames in descending order of similarity (after each first candidate loop closure frame is selected, proceed to steps 5.3-5.6).

[0097] Step 5.3: For the N second three-dimensional points corresponding to the first two-dimensional matrix Each second three-dimensional point in From the N second three-dimensional points corresponding to the selected first candidate loopback frame In the algorithm, the nearest neighbor is found (by minimizing the distance between two points), and this nearest neighbor is called the first nearest neighbor, and is represented as... Then we get N first nearest neighbors. ;

[0098] Step 5.4: Construct the first covariance matrix as follows: ;

[0099] in, , , , ;

[0100] in, The first two-dimensional matrix corresponds to the N points in the second three-dimensional matrix. The centering vector of the second three-dimensional point The first candidate loopback frame selected so far corresponds to the N second three-dimensional points. The centering vector of the second three-dimensional point The center point among the N second three-dimensional points corresponding to the first two-dimensional matrix. The center point is one of the N second three-dimensional points corresponding to the currently selected first candidate loopback frame;

[0101] Step 5.5: Apply the Singular Value Decomposition (SVD) method to the first covariance matrix. Decomposed into The third rotation matrix is ​​obtained. and the third translation vector ;in, The first covariance matrix The left singular vector matrix, The first covariance matrix diagonal matrix, The first covariance matrix The right singular vector matrix, For transpose;

[0102] Step 5.6: Calculate the first error using the N first nearest neighbors, the third rotation matrix, and the third translation vector. Then, it is determined whether the first error is greater than or equal to the first threshold. If it is greater than or equal to the first threshold, the next first candidate loopback frame is selected, and steps 5.3-5.6 are repeated. Otherwise, proceed to step 5.7, and the formula for the first error is as follows:

[0103] ;

[0104] Step 5.7: Calculate the second error using the N first nearest neighbors, the third rotation matrix, and the third translation vector. Then, it is determined whether the second error is greater than or equal to the second threshold. If it is greater than or equal to the second threshold, the next first candidate loopback frame is selected, and steps 5.3-5.7 are repeated. Otherwise, the current first candidate loopback frame is taken as the real first loopback frame, and the formula for the second error is as follows:

[0105] ;

[0106] Step 5.8: Obtain the third rotation matrix and third translation vector corresponding to the actual first loop frame, and use them as the first rotation matrix and first translation vector;

[0107] Similarly, for the two-dimensional matrix received by the cleaning equipment TP1 from the cleaning equipment TP0, the same operation is performed according to steps 5.1-5.8 to obtain the second rotation matrix and the second translation vector:

[0108] That is, in step 5.9, the two-dimensional matrix received by the cleaning equipment TP1 from the cleaning equipment TP0 is called the second two-dimensional matrix. The second two-dimensional matrix is ​​compared with the two-dimensional matrix corresponding to all the historical frame point clouds collected by the cleaning equipment TP1. The two-dimensional matrix corresponding to the K historical frame point clouds with the highest similarity is selected and called the K second candidate loopback frames.

[0109] Step 5.10: Select the K second candidate loop closure frames in descending order of similarity;

[0110] Step 5.11: For the N second three-dimensional points corresponding to the second two-dimensional matrix Each second three-dimensional point in From the N second three-dimensional points corresponding to the selected current second candidate loopback frame In the equation, find the nearest neighbor, which is called the second nearest neighbor, and is represented as... Then we get N second nearest neighbors. ;

[0111] Step 5.12: Construct the second covariance matrix as follows: ;

[0112] in, , , , ;

[0113] in, For the N points in the second three-dimensional matrix corresponding to the second two-dimensional matrix, the first... The centering vector of the second three-dimensional point The second candidate loopback frame is the Nth second three-dimensional point among the currently selected second candidate loopback frames. The centering vector of the second three-dimensional point The center point among the N second three-dimensional points corresponding to the second two-dimensional matrix. The center point is one of the N second three-dimensional points corresponding to the currently selected second candidate loopback frame;

[0114] Step 5.13: Use the Singular Value Decomposition (SVD) method to analyze the second covariance matrix. Decomposed into The fourth rotation matrix is ​​obtained. and the fourth translation vector ;in, The second covariance matrix The left singular vector matrix, The second covariance matrix diagonal matrix, The second covariance matrix The right singular vector matrix;

[0115] Step 5.14: Calculate the third error using the N second nearest neighbors, the fourth rotation matrix, and the fourth translation vector. Then, it is determined whether the third error is greater than or equal to the first threshold. If it is greater than or equal to the first threshold, the next second candidate loopback frame is selected, and steps 5.11-5.14 are repeated; otherwise, proceed to step 5.15. The formula for the third error is as follows:

[0116] ;

[0117] Step 5.15: Calculate the fourth error using the N second nearest neighbors, the fourth rotation matrix, and the fourth translation vector. Then, it is determined whether the second error is greater than or equal to the second threshold. If it is greater than or equal to the second threshold, the next second candidate loopback frame is selected, and steps 5.11-5.15 are repeated. Otherwise, the current second candidate loopback frame is taken as the real second loopback frame, and the formula for the fourth error is as follows:

[0118] ;

[0119] Step 5.16: Obtain the fourth rotation matrix and fourth translation vector corresponding to the actual second loop frame, and use them as the second rotation matrix and second translation vector.

[0120] Step 6: The cleaning equipment TP1 merges (i.e. splices) the N second three-dimensional points corresponding to the two-dimensional matrix transformed into its own coordinate system with the N second three-dimensional points corresponding to all historical frame point clouds of the cleaning equipment TP1 to form the point cloud map of the cleaning equipment TP1.

[0121] Step 7: The cleaning equipment TP1 uses a first rotation matrix and a first translation vector to transform the N second 3D points corresponding to the current frame point cloud, obtaining a first transformed point cloud map. This first transformed point cloud map is then merged into the point cloud map of the cleaning equipment TP1, resulting in a second transformed point cloud map. The second rotation matrix and second translation vector are then used to transform the first point cloud map again, resulting in a second transformed point cloud map. The cleaning equipment TP1 sends the second transformed point cloud map to the cleaning equipment TP0. The cleaning equipment TP0 merges (i.e., stitches) the received second transformed point cloud map with the N second 3D points corresponding to all its historical frame point clouds, forming the point cloud map of the cleaning equipment TP0. For example... Figures 3-4 As shown, Figure 3 A top-down view of the point cloud map of the TP0 equipment used for the cabin cleaning operation. Figure 4This is a 45-degree side view of the point cloud map of the cleaning equipment TP0. The red dots represent the map of N second 3D points corresponding to all historical frame point clouds of the cleaning equipment TP0, the brown dots represent the path of the cleaning equipment TP0, the green dots represent the second converted point cloud map, and the blue dots represent the path of the cleaning equipment TP1.

[0122] This method and device for collaborative positioning and mapping of multiple automated engineering equipment in thermal power plants calculates the positioning and pose of each cleaning engineering equipment by analyzing each frame of point cloud data collected from the equipment. This yields the positioning of each piece of equipment, and a point cloud map is calculated for each frame, thus achieving mapping of all cleaning equipment and improving the operational efficiency of both the equipment and the dock. This solution reduces data transmission between cleaning equipment by using point cloud data, thus reducing communication data pressure. Maps of equipment with higher numbers are converted and merged with those of equipment with lower numbers to form a complete collaborative mapping solution for multiple cleaning equipment.

[0123] In another embodiment, the present invention also provides a collaborative positioning and mapping device for multiple automated engineering equipment in thermal power plants, including a processor and a memory storing a number of computer instructions, wherein the computer instructions, when executed by the processor, implement the steps of the automatic response method that integrates dual word segmentation and iterative feedback.

[0124] For specific limitations on the collaborative positioning and mapping device for multiple automated engineering equipment in thermal power plants, please refer to the limitations on the collaborative positioning and mapping method for multiple automated engineering equipment in thermal power plants mentioned above, which will not be repeated here.

[0125] The memory and processor are electrically connected directly or indirectly to enable data transmission or interaction. For example, these components can be electrically connected to each other via one or more communication buses or signal lines. The memory stores a computer program that can run on the processor, and the processor implements the method of the present invention by running the computer program stored in the memory.

[0126] The memory may be, but is not limited to, Random Access Memory (RAM), Read Only Memory (ROM), Programmable Read-Only Memory (PROM), Erasable Programmable Read-Only Memory (EPROM), Electrically Erasable Programmable Read-Only Memory (EEPROM), etc. The memory is used to store programs, which the processor executes upon receiving execution instructions.

[0127] The processor may be an integrated circuit chip with data processing capabilities. The aforementioned processor can be a general-purpose processor, including a Central Processing Unit (CPU), a Network Processor (NP), etc. It can implement or execute the methods, steps, and logic block diagrams disclosed in the embodiments of this invention. The general-purpose processor can be a microprocessor or any conventional processor.

[0128] It should be understood that, although Figure 1 The steps in the flowchart are shown sequentially as indicated by the arrows, but these steps are not necessarily executed in the order indicated by the arrows. Unless otherwise specified herein, there is no strict order in which these steps are executed, and they can be performed in other orders. Figure 1 At least some of the steps in the process may include multiple sub-steps or multiple stages. These sub-steps or stages are not necessarily completed at the same time, but can be executed at different times. The execution order of these sub-steps or stages is not necessarily sequential, but can be executed in turn or alternately with other steps or at least some of the sub-steps or stages of other steps.

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

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

Claims

1. A method for multi-automatic engineering equipment collaborative positioning and mapping for thermal power plants, characterized in that: The method for multi-automatic engineering equipment cooperative positioning and mapping of thermal power plants comprises the following steps: Step 1, two cleaning engineering equipments in the target area are numbered as TP0 and TP1, each cleaning engineering equipment collects point cloud data of the surrounding environment, the point cloud data contains multiple frames of point cloud, each frame of point cloud is preprocessed, and each frame of preprocessed point cloud contains n first three-dimensional points on the opposite cleaning engineering equipment and N second three-dimensional points other than the opposite cleaning engineering equipment; For each frame of point cloud collected by each cleaning engineering equipment, the following steps are performed: Step 2, each cleaning engineering equipment calculates the positioning and pose of the opposite cleaning engineering equipment through the n first three-dimensional points collected; Step 3, the N second three-dimensional points are converted into a two-dimensional matrix; Step 4, each cleaning engineering equipment transmits the two-dimensional matrix corresponding to the N second three-dimensional points collected by itself to the opposite cleaning engineering equipment, and each cleaning engineering equipment only receives the two-dimensional matrix transmitted by the cleaning engineering equipment with a smaller number than itself, i.e. the cleaning engineering equipment TP1 receives the two-dimensional matrix transmitted by the cleaning engineering equipment TP0 and converts it into its own coordinate system; Step 5, the cleaning engineering equipment TP1 calculates the first rotation matrix and the first translation vector of the two-dimensional matrix corresponding to the current frame of point cloud according to the two-dimensional matrix corresponding to the current frame of point cloud, the received two-dimensional matrix and the two-dimensional matrix corresponding to the historical frame of point cloud, and calculates the second rotation matrix and the second translation vector of the received two-dimensional matrix; Step 6, the cleaning engineering equipment TP1 merges the N second three-dimensional points corresponding to the two-dimensional matrix converted into its own coordinate system with the N second three-dimensional points corresponding to all historical frames of point cloud of the cleaning engineering equipment TP1 to form a point cloud map of the cleaning engineering equipment TP1; Step 7, the cleaning engineering equipment TP1 converts the N second three-dimensional points corresponding to the current frame of point cloud using the first rotation matrix and the first translation vector to obtain a first converted point cloud map, merges the first converted point cloud map into the point cloud map of the cleaning engineering equipment TP1 to obtain a first point cloud map, and converts the first point cloud map using the second rotation matrix and the second translation vector to obtain a second converted point cloud map, the cleaning engineering equipment TP1 sends the second converted point cloud map to the cleaning engineering equipment TP0, and the cleaning engineering equipment TP0 merges the received second converted point cloud map with the N second three-dimensional points corresponding to all historical frames of point cloud to form a point cloud map of the cleaning engineering equipment TP0.

2. The power plant oriented multi-auto engineering equipment collaborative positioning and mapping method of claim 1, wherein: The preprocessing comprises: filtering three-dimensional points whose horizontal distances from the collected stripping engineering equipment exceed a first preset range, and filtering three-dimensional points whose heights exceed a second preset range , wherein the horizontal distance is the distance between the coordinates of the three-dimensional points in the horizontal and vertical coordinates and the collected stripping engineering equipment, and the height is the coordinate of the three-dimensional points in the vertical coordinate, and the coordinates of each stripping engineering equipment in the respective coordinate system are (0, 0, 0). ​ 3. The power plant oriented multi-auto engineering equipment collaborative positioning and mapping method of claim 1, wherein: The calculation of the positioning and pose of the opposite cleaning engineering equipment through the n first three-dimensional points comprises: n first three-dimensional point representations of a current frame point cloud acquired by the stripping engineering equipment TP0 or the stripping engineering equipment TP1 are represented as The first positioning point of the stripping engineering equipment TP1 or the stripping engineering equipment TP0 is ; And the n first three-dimensional point pairs corresponding to the previous frame point cloud collected by the stripping engineering equipment TP0 or the stripping engineering equipment TP1 correspond to the second positioning point of the stripping engineering equipment TP1 or the stripping engineering equipment TP0 , and each positioning point of the stripping engineering equipment TP1 or the stripping engineering equipment TP0 is represented in the stripping engineering equipment TP0 or the stripping engineering equipment TP1 coordinate system; Subtracting the first positioning point from the second positioning point obtains a displacement vector in the coordinate system of the stripping engineering equipment TP0 or the stripping engineering equipment TP1 ; a unit vector of the X axis in the coordinate system of the stripping engineering equipment TP0 or the stripping engineering equipment TP1 through the fifth rotation matrix The unit vector corresponding to the displacement vector is transformed to （1）； wherein the unit vector corresponding to the displacement vector is also represented as: （2）； wherein is the norm of the displacement vector, the fifth rotation matrix is , , and are the heading angle, the pitch angle and the roll angle, respectively, is the rotation matrix around Z, is the rotation matrix around Y, is the rotation matrix around X; wherein, ； ； ； Solving formula (1) and formula (2) together, the pose of the stripping engineering equipment TP1 or the stripping engineering equipment TP0 is obtained .

4. The power plant oriented multi-auto engineering equipment collaborative positioning and mapping method of claim 2, wherein: The conversion of the N second three-dimensional points into a two-dimensional matrix comprises: N second-dimensional points are represented as follows: Converted to dimensions Two-dimensional matrix That is, a two-dimensional matrix The height is Width is and two-dimensional matrix for A grid, each cell being sized as follows: ,in The height of the grid. Let be the width of the grid, and , ; two-dimensional matrix each element in the matrix is , and represents the maximum height among all extreme points in the grid of the th row and the th column , if there is no extreme point in the grid, then ; in, , ,like The value exceeds Scope, or The value exceeds If the range is defined, then the corresponding extreme points will be filtered out. , That is, the second three-dimensional point The corresponding extreme points, , is the second three-dimensional point The horizontal distance from the corresponding cleanup equipment collected. For the second three-dimensional point The relative azimuth angle of the collected cabin cleaning equipment. This is the floor symbol.

5. The power plant oriented multi-auto engineering equipment collaborative positioning and mapping method of claim 4, wherein: The process of converting the two-dimensional matrix received from the cleaning engineering equipment TP0 into the coordinate system of the cleaning engineering equipment TP1 comprises: The cleaning engineering equipment TP1 restores the received two-dimensional matrix into N second three-dimensional points: for each element of the two-dimensional matrix which is not equal to 0 is reduced to a second three-dimensional point : first each element is converted to a pole point and , a second three-dimensional point is then calculated from the pole point and , , ; If then skip the element; The sixth rotation matrix and the sixth translation vector of the TP0 coordinate system of the stripping engineering equipment to the TP1 coordinate system of the stripping engineering equipment are calculated, and the transformation matrix of the TP0 coordinate system of the stripping engineering equipment to the TP1 coordinate system of the stripping engineering equipment is calculated by using the sixth rotation matrix and the sixth translation vector. The restored N second three-dimensional points are converted by the transformation matrix, so that the stripping engineering equipment TP1 converts the two-dimensional matrix received from the stripping engineering equipment TP0 to its own coordinate system.

6. The power plant oriented multi-auto engineering equipment collaborative positioning and mapping method of claim 1, wherein: The stripping engineering equipment TP1 calculates the first rotation matrix and the first translation vector of the two-dimensional matrix corresponding to the current frame point cloud, and calculates the second rotation matrix and the second translation vector of the received two-dimensional matrix, according to the two-dimensional matrix corresponding to the current frame point cloud, the received two-dimensional matrix and the two-dimensional matrix corresponding to the historical frame point cloud, including: Step 5.1, for the two-dimensional matrix corresponding to the current frame point cloud collected by the stripping engineering equipment TP1, called the first two-dimensional matrix, the first two-dimensional matrix is respectively calculated with the two-dimensional matrix corresponding to all historical frame point clouds collected by the stripping engineering equipment TP1, and K two-dimensional matrices corresponding to the highest similarity historical frame point clouds are respectively selected, and called K first candidate loop frames; Step 5.2, according to the similarity from high to low, the K first candidate loop frames are sequentially selected; Step 5.3, for each second three-dimensional point in the N second three- dimensional points corresponding to the first two-dimensional matrix , find the nearest neighbor point from the N second three-dimensional points corresponding to the current first candidate loopback frame , and denote it as , then obtain the N nearest neighbor points ; and ​ Step 5.

4. Construct the first covariance matrix as: ; wherein , , , ; wherein, is a decentered vector of an Nth second three-dimensional point in the N second three-dimensional points corresponding to the first two-dimensional matrix, is a decentered vector of an Nth second three-dimensional point in the N second three-dimensional points corresponding to the first two-dimensional matrix, is a decentered vector of an Nth second three-dimensional point in the N second three-dimensional points corresponding to the first two-dimensional matrix, is a decentered vector of an Nth second three-dimensional point in the N second three-dimensional points corresponding to the first two-dimensional matrix, is a center point in the N second three-dimensional points corresponding to the first two-dimensional matrix, is a center point in the N second three-dimensional points corresponding to the first two-dimensional matrix. Step 5.5: Apply singular value decomposition to the first covariance matrix. Decomposed into The third rotation matrix is ​​obtained. and the third translation vector ;in, The first covariance matrix The left singular vector matrix, The first covariance matrix diagonal matrix, The first covariance matrix The right singular vector matrix; Step 5.

6. Calculate the first error by using the N nearest neighbors, the third rotation matrix and the third translation vector and determine whether the first error is greater than or equal to the first threshold value, if greater than or equal to the first threshold value, select the next first candidate loopback frame, repeat step 5.3-step 5.6, otherwise enter step 5.7, and the formula of the first error is as follows: ； Step 5.

7. Calculate the second error by using the N nearest neighbors, the third rotation matrix and the third translation vector and determine whether the second error is greater than or equal to the second threshold value, if greater than or equal to the second threshold value, select the next first candidate loopback frame, repeat step 5.3-step 5.7, otherwise take the current first candidate loopback frame as the true first loopback frame, and the formula of the second error is as follows: ； Step 5.8, the third rotation matrix and the third translation vector corresponding to the first real loop frame are obtained as the first rotation matrix and the first translation vector; Similarly, for the two-dimensional matrix received by the stripping engineering equipment TP1 from the stripping engineering equipment TP0, the same operation is performed according to steps 5.1-5.8, to obtain the second rotation matrix and the second translation vector.

7. A multi-automatic engineering equipment collaborative positioning and mapping device for thermal power plants, comprising a processor and a memory storing a plurality of computer instructions, characterized in that, The computer instructions are executed by the processor to implement the steps of the method of any one of claims 1 to 6.

Images