A non-cooperative 6D pose estimation method for a beam-end spraying robot
Through the non-coordinated 6D pose estimation method, a three-dimensional model and deep learning algorithm are used, combined with lidar and consumer-grade 3D cameras, high-precision pose estimation and autonomous navigation of beam end spraying operations are achieved, solving the efficiency in the existing technology The problem of low and low accuracy improves the intelligence and convenience of spraying operations.
Patent Information
- Application Number
- CN202411354660.X
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2024-09-27
- Publication Date
- 2024-11-29
- Estimated Expiration
- 2044-09-27
AI Technical Summary
The existing beam end spraying robot position estimation method has low efficiency and low accuracy. It relies on expensive industrial-grade 3D cameras and is easily disturbed by environmental interference. It is difficult to accurately match the non-combination method. The mating method requires fixed markers and has poor convenience.
Using the non-coordinated 6D pose estimation method, the 3D model at the end of the beam is introduced, the area is divided, and the SLAM algorithm is used to perform the SLAM algorithm to create a map. Combined with consumer-grade 3D cameras and deep learning algorithms, the 6D pose at the end of the beam is estimated, reducing the high accuracy. The dependence of point cloud data enables autonomous navigation and automatic pose adjustment.
It realizes low-cost and high-precision pose estimation, improves the intelligence level of spraying operations, solves the problems of low accuracy, low efficiency and high operation difficulty, reduces the dependence on markers and high-precision cameras, and improves spraying Quality and convenience.
Smart Images

Figure CN118887283B_ABST
Abstract
Description
Technical Field
[0001] The present invention belongs to the technical field of robot posture estimation, and in particular, relates to a non-cooperative 6D posture estimation method for a beam end spraying operation robot. Background Art
[0002] The box beams of railways and highways are an important part of transportation infrastructure. Their quality during the construction process has a critical impact on the safety of the transportation system. At present, after the box beams are manufactured in the prefabricated beam yard, spraying at the beam ends is an indispensable process, which will affect the quality of the prefabricated beams. In this context, beam end spraying robots have been widely used. In order to ensure the accuracy of the beam end spraying robots, their posture needs to be estimated.
[0003] For spraying operations, there are currently two main methods for pose estimation: one is a non-cooperative method, which mainly includes template-based methods, matching-based methods, and deep learning-based methods: Template-based methods such as the Linemod method compare the gradient information of the actual image and the model image, and represent the pose of the actual image with the pose of the model image that is most similar to it. This method is inefficient and takes a long time to perform template pairing calculations. Matching-based methods are suitable for objects with rich texture features, such as the ICP algorithm. In actual scenes, precast beams are composed of concrete, and the texture features are not obvious, making it difficult to match. In addition, the algorithm is restricted by the initial pose, and it is easy to fall into the local optimum when the pose is not good. Advanced representative methods based on deep learning include DenseFusion, FundationPose, etc., which simultaneously extract features from point cloud data and RGB information, and then perform pixel-level point-by-point feature fusion. However, the feature vector of the pose predicted by this method comes from the global feature obtained by global pooling, which weakens the feature anti-interference ability to a certain extent. The other is a coordinated method, in which markers are hung at the ends of beams, such as a chessboard, a QR code, etc. The three-dimensional coordinates of multiple feature points on the markers in the world coordinate system and the two-dimensional coordinates in the camera coordinate system are known. The PnP algorithm is used to solve the 3D-2D projection relationship and obtain the camera pose information.
[0004] At the same time, the prior art, such as the Chinese utility model patent application with application number 202322133191.6, discloses a box-type bridge spraying robot, including a driving chassis, a paint system, a bottom rotating platform, a large-amplitude up and down swing mechanism, a large telescopic arm, a small-amplitude up and down swing and vertical lifting mechanism, a vertical turntable, a small telescopic arm, a six-axis mechanical arm and a spray gun. Through the bottom rotating platform, the large-amplitude up and down swing mechanism, the large telescopic arm, the small-amplitude up and down swing and vertical lifting mechanism, the vertical turntable, and the small telescopic arm, the spray gun can be controlled to swing up and down and rotate left and right at a set angle, and move horizontally at a set distance, so that the spraying path can be planned in advance according to the three-dimensional model of the box. Therefore, it has high spraying efficiency, stable spraying quality, and no problem of causing harm to the workers.
[0005] Obviously, the current posture estimation of the robot for spraying at the end of the beam still has the following shortcomings: 1. Although it has liberated manpower to a certain extent, the overall level of intelligence is not high. At present, the spraying equipment is moved to the end of the beam by manual remote control, and then the distance between the nozzle and the beam end is fine-tuned by ultrasonic, laser and other ranging methods. The actual adjustment accuracy is low, the efficiency is low, and the operation is difficult.
[0006] 2. Non-cooperative template matching methods mostly rely on high-precision point cloud data. The point cloud accuracy of 3D structured light cameras is relatively high, generally reaching the millimeter level, but industrial-grade cameras are expensive and difficult to use on a large scale. In addition, the wavelength of most structured light lasers is within the visible light range, and the imaging is easily affected by sunlight. In addition, when spraying at different beam sites, baffles are installed at the ends of the beams. The size and color of the baffles are not fixed, and the shape of the baffles will change the template used for point cloud matching, making it difficult to accurately match.
[0007] 3. The cooperative method uses a consumer-grade low-cost 3D camera to estimate the beam end position, but it requires a fixed marker to be placed at the beam end, which is inconvenient. In addition, after the first spraying, the marker may be stained by the paint, affecting the accuracy of image recognition, making it difficult to accurately estimate the position in the subsequent second spraying. Summary of the invention
[0008] In view of this, in order to solve the problems raised in the above background technology, a non-cooperative 6D pose estimation method for a beam end spraying operation robot is proposed.
[0009] The objective of the present invention can be achieved through the following technical solutions: The present invention provides a non-cooperative 6D pose estimation method for a beam end spraying robot, the method comprising: S1, importing a three-dimensional model of the beam end to be sprayed, identifying the positions of the edges of the beam end from the three-dimensional model, and dividing the beam end into regions accordingly to obtain divided regions.
[0010] S2. Create a map by executing the SLAM algorithm through the laser radar carried on the chassis of the spray robot to obtain a laser SLAM map. Based on the established laser SLAM map, autonomously navigate to the front of the first divided area and preliminarily adjust the posture.
[0011] S3. Use a 3D camera to perform 6D pose estimation.
[0012] S4. Select the first three-dimensional model of the divided area from the three-dimensional model of the beam end to be sprayed, plan the spraying path based on it, control the spray head at the end of the robot arm to move to the area, turn on the spray head, and perform the spraying operation according to the path calculated by the three-dimensional model of the area.
[0013] S5. After the spraying of the first divided area is completed, the spraying robot autonomously navigates to other divided areas one by one, and repeats steps S3 to S5 until the entire beam end is sprayed once.
[0014] S6. Repeat the above process to spray the beam end for the second time. After completion, the spraying robot autonomously navigates to the starting point. At this point, the entire process of the beam end spraying operation is completed.
[0015] Compared with the prior art, the beneficial effects of the present invention are as follows: (1) The present invention provides a non-cooperative, low-cost 6D pose estimation method for beam ends, which does not require the use of markers at the beam ends, nor does it rely on expensive industrial-grade 3D cameras. It only uses consumer-grade depth cameras to achieve high-precision pose estimation and provide visual guidance for spraying robots during spraying operations.
[0016] (2) The present invention realizes the automated spraying of the beam end by autonomously navigating, automatically adjusting the posture and spraying the spraying robot, thereby improving the overall intelligence level of the corresponding operation of the spraying robot and solving the current problems of low actual adjustment accuracy, low efficiency and great difficulty in operation.
[0017] (3) The present invention decomposes the difficult problem of accurate 6D pose estimation based on the characteristics of the railway beam end model, and finally estimates the 6D pose of the beam end based on the camera intrinsic parameters and the coordinates of the feature point P, the straight line equation, and the normal vector, thereby reducing the current dependence on high-precision point cloud data and providing convenience for large-scale use. At the same time, it also avoids the tediousness of replacing the current point cloud matching template, improves the matching accuracy and the accuracy of image recognition, and also improves the convenience of pose estimation and the accuracy and feasibility of pose estimation after secondary spraying. BRIEF DESCRIPTION OF THE DRAWINGS
[0018] In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the accompanying drawings required for describing the embodiments will be briefly introduced below. Obviously, the accompanying drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other accompanying drawings can be obtained based on these accompanying drawings without paying creative work.
[0019] Figure 1 The present invention is a schematic flow chart of the steps for implementing the method.
[0020] Figure 2 Schematic diagram of the three-dimensional model of the beam end.
[0021] Figure 3 Schematic diagram of the 6D pose estimation principle. DETAILED DESCRIPTION
[0022] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.
[0023] See also Figure 1 As shown, the present invention provides a non-cooperative 6D pose estimation method for a beam end spraying robot, and the 6D pose estimation is to calculate the translation matrix and the rotation matrix. The problem of accurate 6D pose estimation can be decomposed into three relatively easy sub-problems: (1) P point coordinate estimation (2) plane 1 normal vector estimation (3) edge 1 straight line estimation. A low-cost 3D camera is used to solve these three sub-problems respectively to complete the pose estimation, wherein the method includes: S1, importing a three-dimensional model of the beam end to be sprayed, identifying the positions of the edges of the beam end from the three-dimensional model, and dividing the beam end into regions based on this to obtain each divided region.
[0024] Specifically, the three-dimensional model of the beam end to be sprayed is as follows: Figure 2 As shown, the baffle is Figure 2 As shown by the closed curve in the figure. In a specific embodiment, the beam end can be divided into three areas, namely area 1, area 2, and area 3 from left to right. In practice, spraying is performed in sequence according to areas 1, 2, and 3. Among them, area 2 is divided from the other two areas by edge 2 and edge 3. And edge 1 is located in the middle and upper part of the box beam, and edge 1 is on the same plane and perpendicular to edge 2 and edge 3.
[0025] S2. Create a map by executing the SLAM algorithm through the laser radar carried on the chassis of the spray robot to obtain a laser SLAM map. Based on the established laser SLAM map, autonomously navigate to the front of the first divided area and preliminarily adjust the posture.
[0026] Specifically, the laser SLAM map provides rough pose information, and the pose can be preliminarily adjusted according to the rough pose information provided. The SLAM algorithm is a relatively mature algorithm currently available. The laser radar executes the SLAM algorithm to create a map. The specific process of obtaining the laser SLAM map will not be repeated here.
[0027] The embodiment of the present invention realizes automated spraying of the beam end by autonomous navigation, automatic posture adjustment and spraying by the spray robot, improves the overall intelligence level of the corresponding operation of the spray robot, and solves the current problems of low actual adjustment accuracy, low efficiency and great operation difficulty.
[0028] S3. Use a 3D camera to perform 6D pose estimation.
[0029] Specifically, the specific estimation principle of 6D pose estimation using a 3D camera is as follows: Figure 3 As shown in the figure, under the premise that the laser SLAM provides a rough pose, a 3D camera is needed to further estimate the fine pose. Observe the features of the beam end. The feature point P is located on the edge 2 of the segmented area 1 and 2. The left and right areas of the edge 2 are not on the same plane, and the image depth features are obvious. The feature point P is also located on plane 1. Combined with the normal vector n of the plane, the three-dimensional coordinates of the feature point P in the camera coordinate system can be calculated to obtain the translation matrix in the pose estimation. The image information of edge 1 and plane 1 is not affected by the secondary spraying, and the depth information features and image gradients above and below edge 1 are obvious. According to plane 1 and edge 1, the rotation matrix in the pose estimation is obtained. In this way, the precise pose that requires a high-precision camera to complete is decomposed into a point, line, and surface estimation problem completed by a low-cost camera, which greatly reduces the requirements for camera performance. Therefore, the 6D pose estimation problem of the beam end can be transformed into: under the premise that the laser SLAM map provides a rough pose, estimate the coordinates of the feature point P, the normal vector of plane 1, and the straight line equation of edge 1, and calculate the translation matrix and rotation matrix.
[0030] Exemplarily, using a 3D camera to perform 6D pose estimation includes: S31, selecting feature points on edge 2, marked as P.
[0031] S32, the spraying robot autonomously navigates to the front of area 1, and uses the onboard 3D camera to shoot the area where the feature point P is located to obtain a captured image.
[0032] S33. Use the deep learning heat map center point detection algorithm to obtain the pixel coordinates of the feature point P.
[0033] Understandably, when detecting the center point, since there is a baffle near the area where the feature point P is located, and the secondary spraying affects the RGB data, and the beam yard is in an outdoor environment, it is easily affected by light, resulting in poor image data. Therefore, the traditional target detection algorithm is difficult to implement. The deep learning algorithm has the advantages of strong anti-noise ability and strong feature extraction ability. Therefore, the deep learning algorithm is used to detect the coordinates of the feature point P. The algorithm mainly includes data preprocessing, feature extraction network and center point detection network.
[0034] It is also understandable that the deep learning algorithm for heat map center point detection can be a convolutional neural network, which can accept an input image and output a heat map, where the peak position on the heat map corresponds to the pixel coordinates of the feature point P. Before training the model, a large amount of annotated data needs to be prepared, that is, the image of the feature point P and the corresponding real pixel coordinate annotation. And this data can be obtained by manual annotation or using other automated annotation methods.
[0035] S34, according to the preset selection area, select a plane on the beam end edge 1, record it as plane 1, calculate the normal vector of plane 1, record it as .
[0036] It should be added that the plane selection on the beam end edge 1 can be performed by determining an area in the three-dimensional model, and the points in the area constitute the plane 1. Alternatively, in the acquired image, the range of the plane 1 can be determined based on the characteristics of the edge 1 and some geometric constraints.
[0037] S35. Based on the 3D camera carried by the local spraying robot, take a picture at the beam end edge 1 again, and use the straight line detection algorithm to obtain the straight line equation of the beam end edge 1.
[0038] It should be added that the RGB-D data of edge 1 is used for line detection. Because the depth data of the upper and lower parts of the edge are quite different and the gradient on the grayscale image is large, a line detection algorithm, referred to as the LSD algorithm, is used for line detection. The core of the algorithm is to find pixels with large gradient changes in the image. Finally, the starting point coordinates, line segment length, line angle, etc. of the line are output to obtain the line equation. The line equation generally adopts the standard first-order equation form, which is an existing equation and the specific formula is not shown here.
[0039] S36, pixel coordinates and plane normal vector based on feature point P , calculate the translation matrix of the beam end in the 6D position in the camera coordinate system, denoted as .
[0040] S37, select a plane perpendicular to plane 1 and whose intersection line is edge 1 as plane 2, and calculate the normal vector of plane 2, which is recorded as , and then calculate and , Mutually perpendicular vectors , we get the rotation matrix of the beam end in the 6D position in the camera coordinate system, which is recorded as .
[0041] S38. Based on the camera intrinsic parameters, translation matrix and rotation matrix, the estimated 6D pose of the beam end is obtained, which is marked as , .
[0042] In a specific embodiment, due to the complex outdoor site environment, the presence of strong light interference, rainy and other adverse weather conditions, the depth information of the beam end obtained by the camera has relatively large noise. When taking pictures of the feature point P, edge 1, and plane 1, they should be photographed from multiple perspectives to obtain multiple sets of RGB-D data, and the multiple processing results should be coherently accumulated to reduce the noise level.
[0043] It can be understood that pose estimation mainly involves data preprocessing, detection algorithm and pose calculation results. The input is the original RGB-D data of the camera. The detection algorithm includes three algorithms: center point detection, plane detection and line detection. According to the different algorithms, data preprocessing processes the input data into grayscale images and point clouds, and then selects different branches for detection. The algorithm corresponds to the coordinates of the output feature point P, the plane normal vector And the regressed straight line equation, finally combined with the camera intrinsic parameters to calculate the translation matrix and rotation matrix to obtain the 6D pose of the beam end.
[0044] Furthermore, it should be added about step S31 that point P is a specific point selected on edge 2, and usually an easily identifiable and representative position is selected. In practical applications, the selection of point P should take into account its visibility and stability so as to facilitate accurate capture and positioning by a visual system. For example, point P can be the end point or center point of the edge, and the present invention uses the center point as point P.
[0045] It should also be added that the role of point P is to provide a clear position reference for subsequent image processing and calculation. For example, by taking a photo of point P and detecting its pixel coordinates, it can be used as a basis for calculating the position of the beam end relative to the camera. Combined with other geometric information, such as the relationship between the plane normal vector and the plane, the overall 6D pose of the beam end can be further inferred.
[0046] Furthermore, the specific process of obtaining the pixel coordinates of the feature point P in step S33 is as follows: J1. Preprocess the acquired image, and extract the RGB-D image data of the feature point P from the preprocessed acquired image.
[0047] It should be added that preprocessing includes some image flipping, scaling, cropping, enhancement, etc.
[0048] J2. Input the RGB-D image data containing the feature point P into a pre-trained feature extraction network to perform feature extraction and obtain the extracted features.
[0049] It should be added that the features include information such as the shape, texture, and color of objects in the image, and in a specific embodiment, the feature extraction network may use MobileNetV2, which has the advantages of being lightweight, small in size, and having a fast detection speed.
[0050] In a specific embodiment, the feature extraction network is mainly composed of a 2D convolution layer, a bottleneck residual block, and an average pooling layer, among which the bottleneck residual is the key part. The channel of the feature map is first expanded through a 1x1 point-by-point convolution operation to enrich the number of features and thereby improve the accuracy. The image is then downsampled through a 3*3Conv+ReLU separable convolution, and finally dimension reduction is performed through a 1*1Conv (without ReLU). For the bottleneck layer, the input and output features are connected only when stride=1.
[0051] J3. Extract high-dimensional feature maps from the extracted features and import them into the center point detection network. The first detection head of the center point detection network performs the first positioning to obtain the preliminary positioning coordinates of the feature point P, which is recorded as .
[0052] The specific positioning process of the first positioning includes: J31, reading the Bbox label data of the feature point P from the preprocessed collected image , and They represent the horizontal and vertical coordinate components of the upper left corner of the bounding box respectively. and They represent the horizontal and vertical coordinate components of the lower right corner of the bounding box respectively.
[0053] It should be added that Bbox label data refers to the location information of the target object in the image, usually expressed in the form of a bounding box. A bounding box is a rectangular area used to select the target object in the image.
[0054] It should also be added that the reason for using the upper left corner coordinates and the lower right corner coordinates of the bounding box to represent the bounding box is that the upper left corner coordinates and the lower right corner coordinates can directly represent the two diagonal points of the rectangular box. Through these two points, the width and height of the rectangular box, as well as the center point coordinates, can be easily calculated, which also facilitates operations such as image enhancement.
[0055] J32. Calculate the position coordinates of the center point, recorded as , .
[0056] J33, set the downsampling factor, denoted as , then the coordinates of the center point after downsampling are , .
[0057] It should be added that large-size images consume a lot of computing resources when training and reasoning deep learning models such as convolutional neural networks. By downsampling, the size of the image can be reduced, thereby reducing the computational complexity and accelerating the processing speed, and the downsampling factor can be 4.
[0058] J34. Extract the size from the preprocessed collected image and set the heat map size accordingly, denoted as , and Represents the width and height of the image respectively, and creates a The full zero matrix of is used as a heat map.
[0059] J35. In the heat map Generate a Gaussian distribution at the position and apply it to the heat map Position, and the peak of the Gaussian distribution is located at The center point coordinates are obtained from the heat map as the preliminary positioning coordinates of the feature point P.
[0060] It should be added that the Gaussian distribution formula is: , Represents the coordinates of any position on the heat map, Represents the target scale adaptive standard deviation, where the heat map value range is 0 to 1. When the value is 1, it means that this point is the center point of the target, that is, the feature point P.
[0061] It should also be added that the target scale adaptive standard variance means that the size of the variance will be adjusted according to the scale of the target. This makes the Gaussian distribution more suitable for the actual size of the target, thereby more accurately representing the influence range of the target center point on the heat map.
[0062] J4. Based on the preliminary positioning coordinates of the feature point P, the second detection head of the center point detection network performs secondary positioning to obtain the pixel coordinates of the feature point P in the captured image.
[0063] The specific positioning process of the secondary positioning includes: J41, setting the selection distance, recorded as , the distance from the initial positioning coordinate position of the feature point P in the high-dimensional feature map is area as a candidate area.
[0064] In a specific embodiment, The specific value may be 10 to 30. For example, The value can be 30, where 30 refers to the pixel value.
[0065] J42. Calculate the size of the ROI area, and based on the size of the ROI area, crop the ROI area from the candidate area of the high-dimensional feature map, and input the cropped ROI area into the second detection head.
[0066] In a specific embodiment, the size of the ROI area is 64*64*4, and the boundary of the ROI area needs to be considered in the actual process. When the size of the high-dimensional feature map is insufficient, boundary processing such as filling is required to extract the preliminary positioning coordinates of the feature point P. , that is, the coordinate of the upper left corner after filling is , the coordinates of the lower right corner after filling are .
[0067] J43, record the offset coordinates output by the second detection head as .
[0068] J44. Preliminary positioning coordinates based on feature point P and offset , the pixel coordinates of the feature point P in the captured image can be calculated , .
[0069] It should also be added that the first detection head performs the first positioning and the second detection head performs the second positioning, and the loss function is set. The specific setting of the loss function is as follows: the category weight and the adjustment coefficient are set, which are respectively recorded as and .
[0070] Set the loss function as ,
[0071] , represents the total number of training samples, For the The heat map value of the center point under each category, Expressed as The heat map value of the center point predicted under the category, Indicates the category number, .
[0072] It should be added that category weights are mainly used to deal with category imbalance problems, that is, the number of samples in some categories is much larger than that in other categories. Category weights can be set according to the number of samples or task requirements. The adjustment coefficient is used to balance the importance of different tasks or loss items. The adjustment coefficient can be set according to the importance of the task and the performance of the model. The specific values of category weights and adjustment coefficients need to be determined according to the characteristics of the specific task and data set. In an actual scenario, they can be comprehensively determined through multiple methods such as grid search and cross-validation.
[0073] Further, regarding the calculation of the normal vector of plane 1 in step S34, it includes: extracting the point cloud data of plane 1, using the Ransac algorithm to perform point cloud plane regression, and obtaining the fitting equation corresponding to plane 1, wherein the fitting equation corresponding to plane 1 is , and are the variables in the fitting equation, which correspond to each coordinate component one by one. is a constant, , and is the coefficient corresponding to each variable, recorded as each coefficient.
[0074] Extract the coefficients of the fitting equation corresponding to plane 1, which are recorded as , and , calculate the normal vector of plane 1 , Represents the transpose symbol.
[0075] In another specific embodiment, the normal vector of plane 1 can also be calculated by arbitrarily selecting three non-collinear points on plane 1. The specific calculation process is: the three selected non-collinear points are respectively recorded as point F, point V and point Q.
[0076] Let the coordinates of point F be , and mark the coordinates of point V as , and mark the coordinates of point Q as ;
[0077] Calculate vector FV and vector FQ, where, ,
[0078] , and then calculate the normal vector of plane 1 , .
[0079] Further, regarding the calculation of the translation matrix of the beam end in the 6D position in the camera coordinate system in step S36, it includes: marking the coordinates of the feature point P in the edge 2 in the camera coordinate system as , the homogeneous coordinates are denoted by , At the same time, the coordinates of the feature point P in edge 2 in the camera coordinate system are marked as ,
[0080] .
[0081] The camera intrinsic parameter matrix is recorded as , using the camera intrinsic parameter matrix , convert the homogeneous coordinates into the coordinates in the camera coordinate system, .
[0082] according to The conditions on plane 1 can be calculated as , and then calculate The matrix of , and used as the translation matrix of the beam end in the 6D pose in the camera coordinate system, Represents a constant.
[0083] Further, regarding the calculation of the normal vector of plane 2 in step S37, the method includes: selecting two points at random in edge 1 and recording them as and , set the coordinates of feature point P in edge 2 in the same way as in the camera coordinate system and The coordinates of the two points in the camera coordinate system.
[0084] based on and The coordinates of the two points in the camera coordinate system are used to construct the straight line equation of edge 1 in the camera coordinate system and set the scale factor, which is recorded as .
[0085] Based on the normal vector of plane 1, the straight line equation of plane 1 is constructed, and based on the straight line equation of edge 1 in the camera coordinate system and the straight line equation of plane 1, the coordinates of the intersection point between edge 1 and plane 1 are calculated.
[0086] Calculate the direction vector of edge 1, and use the product of the direction vector of edge 1 and the normal vector of plane 1 as the normal vector of plane 2.
[0087] It should be added that the straight line equation of edge 1 in the camera coordinate system can be used to determine any point on the straight line. When , the point on the line corresponds to the point .when When , the point on the line corresponds to the point For other values, we can get a straight line between and By changing The value of can be used to perform parameterized movement on the straight line, thereby describing the entire straight line of edge 1 in the camera coordinate system.
[0088] In a specific embodiment, the detailed supplementary process of calculating the normal vector of plane 2 is as follows: The coordinates in the camera coordinate system are ,Will The coordinates in the camera coordinate system are .
[0089] Construct the straight line equation of edge 1 in the camera coordinate system, which is specifically expressed as follows:
[0090] , ,
[0091] .
[0092] Construct the equation of the line in plane 1:
[0093] , Indicates the coordinates of the set point on plane 1, which are fixed coordinates. Shows the coordinates of any point on plane 1.
[0094] Substitute the straight line equation of edge 1 in the camera coordinate system into the straight line equation of plane 1 and calculate the proportional coefficient Solve, and then solve Substitute the straight line equation of edge 1 in the camera coordinate system to obtain the coordinates of the intersection point.
[0095] Calculate the direction vector of edge 1. The direction vector of edge 1 is ,Will Recorded as ,Will Recorded as ,Will Recorded as . Calculate the normal vector of plane 2 ,
[0096] .
[0097] It should also be added that the process of solving the equation of a straight line is a mathematical process, which will not be described in detail or supplemented here.
[0098] Furthermore, the specific process of obtaining the rotation matrix of the beam end in the 6D position in the camera coordinate system in step S37 is as follows: , normal vector and vector Normalize the normalized vector , normal vector and vector Denoted as , and .
[0099] Construct the rotation matrix of the beam end in the 6D pose in the camera coordinate system, denoted as , .
[0100] It should be added that the normal vector , normal vector and vector Normalization is a common method. Normalizing a vector means converting it into a vector with the same direction but a length of 1. The normalization formula is no longer shown.
[0101] The present invention designs a high-precision 6D pose estimation algorithm based on the characteristics of the railway beam end model. The characteristic area on the beam is used for accurate pose estimation, and then the center point P coordinate of the straight line is detected with high precision through the deep learning heat map detection method, and then the LSD algorithm is used to detect the straight line of edge 1 to obtain the straight line equation, and then the Ransac algorithm is used to perform point cloud regression of plane 1 to obtain the plane normal vector. Finally, according to the camera internal parameters and the characteristic point P coordinates, the straight line equation, and the normal vector, the effective decomposition of the difficult and accurate 6D pose estimation problem is achieved, and then the 6D pose of the beam end is estimated, which reduces the current dependence on high-precision point cloud data, provides convenience for large-scale use, and also avoids the cumbersomeness of the current point cloud matching template replacement, improves the matching accuracy and the accuracy of image recognition, and also improves the convenience of pose estimation and the accuracy and feasibility of pose estimation after secondary spraying.
[0102] S4. Select the first three-dimensional model of the divided area from the three-dimensional model of the beam end to be sprayed, plan the spraying path based on it, control the spray head at the end of the robot arm to move to the area, turn on the spray head, and perform the spraying operation according to the path calculated by the three-dimensional model of the area.
[0103] It should be added that the spraying path can be planned by a path planning algorithm. For example, the spraying path can be generated by using an A* algorithm or an RRT algorithm.
[0104] S5. After the spraying of the first divided area is completed, the spraying robot autonomously navigates to other divided areas one by one, and repeats steps S3 to S5 until the entire beam end is sprayed once.
[0105] S6. Repeat the above process to spray the beam end for the second time. After completion, the spraying robot autonomously navigates to the starting point. At this point, the entire process of the beam end spraying operation is completed.
[0106] The embodiment of the present invention provides a non-cooperative, low-cost 6D pose estimation method for the beam end. It does not require the use of markers at the beam end, nor does it rely on expensive industrial-grade 3D cameras. It only uses consumer-grade depth cameras to achieve high-precision pose estimation and provide visual guidance for the spraying operation of the spray robot.
[0107] The above contents are merely examples and explanations of the concept of the present invention. Those skilled in the art may make various modifications or additions to the specific embodiments described or replace them in a similar manner. As long as they do not deviate from the concept of the invention or exceed the scope defined by the present invention, they shall all fall within the protection scope of the present invention.
Claims
1. A non-cooperative 6D pose estimation method for a beam end spraying robot, characterized in that: The method includes: S1, importing a three-dimensional model of the beam end to be sprayed, identifying the positions of the edges of the beam end from the three-dimensional model, and dividing the beam end into regions based on the positions to obtain divided regions; S2, create a map by executing the SLAM algorithm through the laser radar carried on the chassis of the spray robot, obtain the laser SLAM map, and autonomously navigate to the front of the first divided area based on the established laser SLAM map, and preliminarily adjust the posture; S3, use 3D camera to perform 6D pose estimation; S4, selecting the first three-dimensional model of the divided area from the three-dimensional model of the beam end to be sprayed, planning the spraying path based on the model, controlling the spray head at the end of the robot arm to move to the area, turning on the spray head, and performing the spraying operation according to the path calculated by the three-dimensional model of the area; S5. After the first divided area is sprayed, the spraying robot autonomously navigates to other divided areas one by one, and repeats steps S3 to S5 until the entire beam end is sprayed at one time; S6, repeating steps S2 to S5 to perform secondary spraying on the beam end. After completion, the spraying robot autonomously navigates to the starting point, and the entire process of the beam end spraying operation is completed; The 6D pose estimation using a 3D camera includes: The first divided area is marked as area 1, and the analysis edges are selected from the three-dimensional model of the beam end to be sprayed, which are marked as edge 1, edge 2 and edge 3 respectively, and the feature points are selected on edge 2 and marked as ; The spraying robot autonomously navigates to the front of area 1 and uses the 3D camera on board to capture feature points The area where the image is located is obtained; Use the deep learning heat map center point detection algorithm to obtain feature points The pixel coordinates of According to the preset selection area, a plane is selected on the beam end edge 1, which is recorded as plane 1. The normal vector of plane 1 is calculated and recorded as ; Based on the 3D camera carried by the local spray robot, take pictures again at the beam end edge 1, and use the straight line detection algorithm to obtain the straight line equation of the beam end edge 1; Based on feature points The pixel coordinates and plane normal vector , calculate the translation matrix of the beam end in the 6D position in the camera coordinate system, denoted as ; Select the plane that is perpendicular to plane 1 and whose intersection line is edge 1 as plane 2, and calculate the normal vector of plane 2, which is recorded as , and then calculate and , Mutually perpendicular vectors , we get the rotation matrix of the beam end in the 6D position in the camera coordinate system, which is recorded as ; Based on the camera intrinsic parameters, translation matrix and rotation matrix, the estimated 6D pose of the beam end is obtained, which is marked as , ; The calculation of the normal vector of plane 1 includes: Extract the point cloud data of plane 1, use the Ransac algorithm to perform point cloud plane regression, and obtain the fitting equation corresponding to plane 1; Extract the coefficients of the fitting equation corresponding to plane 1, which are recorded as , and , calculate the normal vector of plane 1 , represents the transpose symbol; The calculation of the translation matrix of the beam end in the 6D pose in the camera coordinate system includes: The feature points in edge 2 The coordinates of , the homogeneous coordinates are denoted by , , and at the same time, the feature points in edge 2 in the camera coordinate system Coordinates are marked as , ; The camera intrinsic parameter matrix is recorded as , using the camera intrinsic parameter matrix , convert the homogeneous coordinates into the coordinates in the camera coordinate system, ; according to The conditions on plane 1 can be calculated as , and then calculate The coordinates of , and used as the translation matrix of the beam end in the 6D pose in the camera coordinate system, represents a constant; The calculation of the normal vector of plane 2 includes: Select any two points in edge 1 and record them as and , according to the feature points in edge 2 in the camera coordinate system The coordinates are set in the same way and The coordinates of the two points in the camera coordinate system; based on and The coordinates of the two points in the camera coordinate system are used to construct the straight line equation of edge 1 in the camera coordinate system and set the scale factor, which is recorded as ; Based on the normal vector of plane 1, the straight line equation of plane 1 is constructed, and based on the straight line equation of edge 1 in the camera coordinate system and the straight line equation of plane 1, the coordinates of the intersection point between edge 1 and plane 1 are calculated; Calculate the direction vector of edge 1, and use the product of the direction vector of edge 1 and the normal vector of plane 1 as the normal vector of plane 2.
2. The non-cooperative 6D pose estimation method of a beam end spraying robot according to claim 1, characterized in that: The feature points The specific process of obtaining the pixel coordinates is as follows: Preprocess the collected image and extract feature points from the preprocessed collected image RGB-D image data; Will contain feature points The RGB-D image data is input into a pre-trained feature extraction network to perform feature extraction and obtain the extracted features; A high-dimensional feature map is extracted from the extracted features, and the features are imported into the center point detection network. The first detection head of the center point detection network performs the first positioning to obtain the feature points. The initial positioning coordinates are recorded as ; Based on feature points The initial positioning coordinates are then re-positioned by the second detection head of the center point detection network to obtain the feature points The pixel coordinates in the acquired image.
3. The non-cooperative 6D pose estimation method of a beam end spraying robot according to claim 2, characterized in that: The specific positioning process of the first positioning includes: Read feature points from the preprocessed captured image Bbox label data , and They represent the horizontal and vertical coordinate components of the upper left corner of the bounding box respectively. and Respectively represent the horizontal and vertical coordinate components of the lower right corner of the bounding box; Calculate the position coordinates of the center point, recorded as , ; Set the downsampling factor, denoted as , then the coordinates of the center point after downsampling are , ; Extract the size from the preprocessed collected image and set the heat map size accordingly, denoted as , and Represents the width and height of the image respectively, and creates a The all-zero matrix of is used as a heat map; In the heat map Generate a Gaussian distribution at the position and apply it to the heat map Position, and the peak of the Gaussian distribution is located at Position, and then obtain the center point coordinates from the heat map as the feature point The initial positioning coordinates.
4. The non-cooperative 6D pose estimation method of a beam end spraying robot according to claim 2, characterized in that: The specific positioning process of the secondary positioning includes: Set the selection distance, denoted as , in the high-dimensional feature map, the distance feature point The distance from the initial positioning coordinate position is Region as a candidate region; Calculate the size of the ROI area, and based on the size of the ROI area, crop the ROI area from the candidate area of the high-dimensional feature map, and input the cropped ROI area into the second detection head; The offset coordinates output by the second detection head are recorded as ; Based on feature points The initial positioning coordinates and offset , the feature points can be calculated Pixel coordinates in the acquired image , .
5. The non-cooperative 6D pose estimation method of a beam end spraying robot according to claim 2, characterized in that: The first detection head performs the initial positioning and the second detection head performs the secondary positioning, and the loss function is also set, where: The specific settings for setting the loss function are as follows: Set the category weight and adjustment coefficient, respectively and ; Set the loss function as , , represents the total number of training samples, For the The heat map value of the center point under each category, Expressed as The heat map value of the center point predicted under the category, Indicates the category number, .
6. The non-cooperative 6D pose estimation method of a beam end spraying robot according to claim 1, characterized in that: The specific process of obtaining the rotation matrix of the beam end in the 6D pose in the camera coordinate system is as follows: The normal vector , normal vector and vector Normalize the normalized vector , normal vector and vector Denoted as , and ; Construct the rotation matrix of the beam end in the 6D pose in the camera coordinate system, denoted as , .
Citation Information
Patent Citations
Box type bridge spraying operation robot
CN220461096U
Three-dimensional scanning data and BIM model nesting method
CN115270249A
Putty coating spraying robot based on BIM technology
CN115949210A
