A method, system and equipment for positioning a power infrastructure leveling robot.

Abstract

This invention discloses a method, system, and device for locating a leveling robot in power infrastructure construction, relating to the field of robot localization technology. The method includes: acquiring a target image of a pre-set target; determining the first pose of the leveling robot based on the target image and pre-acquired camera internal parameters; determining the second pose of the leveling robot based on an inertial sensor; and then, under different motion states of the leveling robot, employing different Kalman filtering methods, fusing the first and second poses using Kalman filtering to complete the autonomous localization of the leveling robot. This invention can improve the accuracy of leveling robot localization.

Description

Technical Field

[0001] This invention relates to the field of electric robot technology, and in particular to a positioning method, system and equipment for a concrete floor leveling robot used in power infrastructure construction. Background Technology

[0002] In power infrastructure construction, leveling concrete floors is a crucial and complex task. Currently, construction projects generally rely on manual labor or traditional, human-operated concrete floor leveling machines. However, manual labor is labor-intensive, inefficient, poses safety hazards, and yields less than ideal leveling results. Therefore, high-quality, high-efficiency, and intelligent concrete floor leveling machines have emerged.

[0003] Intelligent concrete floor leveling machines are designed to automatically level concrete floors during construction. Before performing the leveling action, the machine's position and posture on the construction site must be obtained at the current moment. Then, the machine's trajectory is planned and leveling motion is performed based on the position and posture to avoid leveling areas that do not need to be leveled.

[0004] Existing technologies mainly utilize cameras to locate leveling machines. By using a camera mounted on the leveling machine and a target placed at a designated location, the machine's position on the construction site at the current moment can be determined. However, this positioning method has drawbacks. It can only acquire target images and perform positioning effectively when the leveling machine's movement is stable or the camera is not far from the target. Once the leveling machine experiences significant shaking or the camera is far from the target, it is difficult for the camera to obtain effective recognition images, resulting in inaccurate positioning. Summary of the Invention

[0005] This invention provides a positioning method, system, and equipment for power infrastructure leveling robots, which solves the technical problem of poor positioning effect of existing leveling machine positioning methods.

[0006] The first aspect of this application provides a positioning method for a power infrastructure leveling robot, including:

[0007] The target image of the preset target is obtained by a camera set on the leveling robot;

[0008] Based on the target image and the pre-acquired internal parameters of the camera, the first pose of the leveling robot is determined; the distance between the camera and the preset target is determined based on the first pose.

[0009] The second pose of the leveling robot is determined by an inertial sensor mounted on the robot.

[0010] Determining the pose of the leveling robot based on the distance, the first pose, and the second pose includes:

[0011] When the distance exceeds the first threshold, the second pose is used as the state vector of the leveling robot, the first pose is used as the observation vector of the leveling robot, and an adaptive Kalman filter is applied to the first pose and the second pose to obtain the pose of the leveling robot.

[0012] When the distance does not exceed a first threshold time, the acceleration change rate and yaw angle change rate of the leveling robot are obtained based on the inertial sensor. When the acceleration change rate exceeds a second threshold or the yaw angle change rate exceeds a third threshold, the second pose is used as the state vector of the leveling robot, and the first pose is used as the observation vector of the leveling robot. An adaptive Kalman filter is applied to the first pose and the second pose to obtain the pose of the leveling robot. When the acceleration change rate does not exceed the second threshold and the yaw angle change rate does not exceed the third threshold, the first pose is used as the state vector of the leveling robot, and the second pose is used as the observation vector of the leveling robot. An unscented Kalman filter is applied to the first pose and the second pose to obtain the pose of the leveling robot.

[0013] Preferably, acquiring the target image of the preset target using a camera mounted on the leveling robot includes:

[0014] A first image of the site to be leveled is acquired using a camera mounted on the leveling robot;

[0015] The first image is filtered based on a preset filtering method to obtain a first processed image;

[0016] The first processed image is enhanced based on a preset enhancement method to obtain a second processed image;

[0017] The second processed image is segmented based on a preset segmentation method to obtain a target image of a preset target; wherein the target image contains a number of target points.

[0018] Preferably, determining the first pose of the leveling robot based on the target image and the pre-acquired internal parameters of the camera includes:

[0019] Determine the first coordinates of the target point in the pixel coordinate system;

[0020] Determine the second coordinates of the target point in the world coordinate system;

[0021] Based on the pre-acquired internal parameters of the camera, the first coordinate, and the second coordinate, the first pose of the leveling robot is determined using bundle adjustment.

[0022] Preferably, the step of segmenting the second processed image based on a preset segmentation method to obtain a target image of a preset target includes:

[0023] The second processed image is subjected to thresholding and Canny edge detection to obtain a target image of the preset target.

[0024] Preferably, determining the first coordinates of the target point in the pixel coordinate system includes:

[0025] The target center of the target point is determined based on the centroid method and the least squares fitted circle method, and the position of the target center in the pixel coordinate system is recorded as the first coordinate of the target point.

[0026] Preferably, the preset filtering method is mean filtering, Gaussian filtering, or median filtering.

[0027] Preferably, the preset enhancement method is grayscale transformation.

[0028] The second aspect of this application provides a positioning system for a power infrastructure leveling robot, comprising:

[0029] The image acquisition module is used to acquire target images of preset targets through a camera set on the leveling robot;

[0030] The first calculation module is used to determine the first pose of the leveling robot based on the target image and the pre-acquired internal parameters of the camera; and to determine the distance between the camera and the preset target based on the first pose.

[0031] The second calculation module is used to determine the second pose of the leveling robot by means of an inertial sensor mounted on the leveling robot.

[0032] The positioning module is used to determine the pose of the leveling robot based on the distance, the first pose, and the second pose; specifically, the positioning module is used for:

[0033] When the distance exceeds the first threshold, the second pose is used as the state vector of the leveling robot, the first pose is used as the observation vector of the leveling robot, and an adaptive Kalman filter is applied to the first pose and the second pose to obtain the pose of the leveling robot.

[0034] When the distance does not exceed a first threshold time, the acceleration change rate and yaw angle change rate of the leveling robot are obtained based on the inertial sensor. When the acceleration change rate exceeds a second threshold or the yaw angle change rate exceeds a third threshold, the second pose is used as the state vector of the leveling robot, and the first pose is used as the observation vector of the leveling robot. An adaptive Kalman filter is applied to the first pose and the second pose to obtain the pose of the leveling robot. When the acceleration change rate does not exceed the second threshold and the yaw angle change rate does not exceed the third threshold, the first pose is used as the state vector of the leveling robot, and the second pose is used as the observation vector of the leveling robot. An unscented Kalman filter is applied to the first pose and the second pose to obtain the pose of the leveling robot.

[0035] Preferably, the image acquisition module is specifically used for:

[0036] A first image of the site to be leveled is acquired using a camera mounted on the leveling robot;

[0037] The first image is filtered based on a preset filtering method to obtain a first processed image;

[0038] The first processed image is enhanced based on a preset enhancement method to obtain a second processed image;

[0039] The second processed image is segmented based on a preset segmentation method to obtain a target image of a preset target; wherein the target image contains a number of target points.

[0040] A third aspect of this application provides a positioning device for a power infrastructure leveling robot, comprising: a memory and a processor;

[0041] The memory is used to store computer programs;

[0042] The processor is configured to implement, when executing the computer program, a power infrastructure leveling robot positioning method as provided in the first aspect of this application.

[0043] The power infrastructure leveling robot positioning method provided by the above-mentioned technical solution of this application has the following advantages: Considering the limitations of determining the pose of the leveling robot based on image processing and based on inertial sensors, when the distance between the preset target and the leveling robot exceeds a first threshold, the second pose determined by the inertial sensor with higher positioning accuracy is used as the state vector of the leveling robot, and the first pose determined by the camera with lower positioning accuracy is used as the observation vector of the leveling robot. Adaptive Kalman filtering is used to determine the pose of the leveling robot; when the distance between the preset target and the leveling robot does not exceed the first threshold, the motion state of the leveling robot is further judged. When the leveling robot moves smoothly, the first pose determined by the camera with high positioning accuracy is used as the state vector of the leveling robot, and the second pose determined by the inertial sensor with lower positioning accuracy is used as the observation vector of the leveling robot. Unscented Kalman filtering is used to determine the pose of the leveling robot. When the leveling robot experiences significant shaking, the second pose determined by the inertial sensor with high positioning accuracy is used as the state vector of the leveling robot, and the first pose determined by the camera with lower positioning accuracy is used as the observation vector of the leveling robot. Adaptive Kalman filtering is used to determine the pose of the leveling robot. This avoids the limitation that accurate positioning can only be achieved when the leveling robot is in a stable motion state. Attached Figure Description

[0044] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0045] Figure 1 A flowchart illustrating the positioning method for a power infrastructure leveling robot provided in this application embodiment;

[0046] Figure 2 This is a schematic diagram of radial image distortion caused by the lens shape, provided as an embodiment of this application.

[0047] Figure 3 This is a schematic diagram of image tangential distortion caused by the lens shape, provided as an embodiment of this application. Detailed Implementation

[0048] To make the objectives, features, and advantages of this invention more apparent and understandable, the technical solutions of the embodiments of this invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the embodiments described below are only some embodiments of this invention, and not all embodiments. Based on the embodiments of this invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of this invention.

[0049] Embodiment 1 of this application provides a positioning method for a power infrastructure leveling robot. Please refer to [link / reference]. Figure 1 In Example 1, it includes:

[0050] S1. Obtain a target image of a preset target using a camera mounted on the leveling robot.

[0051] Specifically, the camera is set in the direction of the leveling robot's movement. The image captured by the camera is a grayscale image. By acquiring the first image of the site to be leveled, and then performing image filtering, image enhancement, and image segmentation on the first image, the target image is obtained.

[0052] To ensure positioning accuracy, in this embodiment, the size of the preset target must meet the following requirement: when the camera is at its farthest visual distance from the preset target in the leveling area, the target image occupies one-tenth of the size of the first image, ensuring that the camera can extract effective feature points of the preset target from any acquired first image. Simultaneously, to reduce the difficulty of target recognition, the preset target in this embodiment is an active target containing LED lights, consisting of an outer rectangular outline and several circular target points of equal size and fixed positions. During target image recognition, recognizing the rectangular outline effectively eliminates interference caused by environmental factors in the image, while also facilitating the determination of the target's position in the image, thus improving recognition accuracy.

[0053] It is important to note that image quality is related to the shooting angle. When the camera on the leveling robot is far from the preset target, or when the angle between the camera and the preset target is large, the target image occupies fewer pixels in the first image or may not even appear in the first image. This makes it impossible to determine the pose of the leveling robot based on the target image, resulting in a visual blind spot. To reduce the impact of the blind spot, this embodiment sets up four preset targets with different patterns and encodes them. The four preset targets are then set up in four directions of the area to be leveled according to their codes.

[0054] Since the four preset target patterns are different, the target code currently being identified can be determined by detecting the target image, and the current orientation of the camera in the area to be leveled can be determined, providing a reference for subsequently determining the pose of the leveling robot in the area to be leveled.

[0055] S2. Determine the first pose of the leveling robot based on the target image and the pre-acquired internal parameters of the camera; determine the distance between the camera and the preset target based on the first pose.

[0056] Specifically, based on the pre-acquired internal parameters of the camera, the first coordinate, and the second coordinate, a preset improved PNP algorithm is used to determine the first pose of the leveling robot. Then, the distance between the camera and the preset target is determined based on the first pose. It can be understood that based on the aforementioned determined target image, the currently identified target code can be determined, thus revealing the position of the preset target in the world coordinate system. Combined with the acquired first pose of the leveling robot in the world coordinate system, the distance between the preset target and the leveling robot can be calculated.

[0057] S3. Determine the second pose of the leveling robot based on the inertial sensors installed on the leveling robot.

[0058] When the camera operates in a slow, stable motion, it can effectively acquire target images and perform localization. However, when the leveling robot rotates or experiences significant shaking, the camera struggles to obtain effective recognition images. In such situations, inertial sensors can better perceive changes in the robot's motion. For the visual positioning system of the power infrastructure leveling robot, relying solely on the camera for localization is insufficient; using inertial sensors as an aid is a sound solution. Therefore, this embodiment simultaneously utilizes inertial sensors to determine the second pose of the leveling robot.

[0059] The inertial sensor consists of an accelerometer and a gyroscope, which performs real-time positioning by acquiring the current rate of change of acceleration and yaw angle of the leveling robot.

[0060] S4. Determine the pose of the leveling robot based on the distance, the first pose, and the second pose; specifically:

[0061] When the distance exceeds the first threshold, the second pose is used as the state vector of the leveling robot, the first pose is used as the observation vector of the leveling robot, and an adaptive Kalman filter is applied to the first pose and the second pose to obtain the pose of the leveling robot.

[0062] The Kalman filter is a highly efficient recursive filter that uses past values ​​and filter coefficients to calculate the current output value through a linear combination.

[0063] It can estimate the true state of a dynamic system relatively well from a series of incomplete and noisy measurements. It consists of two processes: prediction and correction. In the prediction phase, the estimated value from the previous state is used as a parameter to predict the current state value. In the correction phase, the predicted value from the prediction phase is corrected using observations of the current state. Through weight adjustment, a new estimate that is closer to the truth is obtained through linear calculation.

[0064] When the distance between the preset target and the leveling robot exceeds a first threshold, it indicates that the distance between them is too large. In this case, the accuracy of the camera's visual positioning is poor, and the first pose determined based on the target image acquired by the camera may have a large error, affecting the subsequent positioning of the leveling robot. Inertial sensors, composed of accelerometers and gyroscopes, are not limited by distance and are highly sensitive to changes in system state. Therefore, in this embodiment, when the actual distance between the camera and the target is too large, the second pose determined by the inertial sensor is used as the state vector of the leveling robot, and the first pose determined by the camera is used as the observation vector of the leveling robot. Adaptive Kalman filtering is applied to both the first and second poses to determine the pose of the leveling robot.

[0065] When the distance does not exceed a first threshold time, the acceleration change rate and yaw angle change rate of the leveling robot are obtained based on the inertial sensor. When the acceleration change rate exceeds a second threshold or the yaw angle change rate exceeds a third threshold, the second pose is used as the state vector of the leveling robot, and the first pose is used as the observation vector of the leveling robot. An adaptive Kalman filter is applied to the first pose and the second pose to obtain the pose of the leveling robot. When the acceleration change rate does not exceed the second threshold and the yaw angle change rate does not exceed the third threshold, the first pose is used as the state vector of the leveling robot, and the second pose is used as the observation vector of the leveling robot. An unscented Kalman filter is applied to the first pose and the second pose to obtain the pose of the leveling robot.

[0066] Because inertial sensors accumulate errors over long periods of operation, when the distance between the preset target and the leveling robot does not exceed the first threshold, it indicates that the distance between the preset target and the leveling robot is small. At this time, the accuracy of the camera's visual positioning is high, and the first pose determined based on the target image acquired by the camera has virtually no error and will not affect the subsequent positioning of the leveling robot. To avoid the cumulative error caused by the long-term operation of the inertial sensor affecting the accuracy of the leveling robot's positioning, in this embodiment, when the actual distance between the camera and the target is small, the second pose determined by the inertial sensor can be used as the observation vector of the leveling robot, and the first pose determined based on the camera can be used as the state vector of the leveling robot to determine the pose of the leveling robot.

[0067] However, it should be noted that although the camera's visual positioning accuracy is high when the distance between the preset target and the leveling robot does not exceed the first threshold, significant errors can occur under certain special circumstances (rotation of the leveling machine or large-amplitude shaking), making the image obtained by the camera unusable as a valid identification basis and affecting the accuracy of determining the leveling robot's pose. Therefore, in this embodiment, when it is determined that the distance between the preset target and the leveling robot does not exceed the first threshold, the motion state of the leveling robot is further determined. Based on the inertial sensor, the acceleration change rate and yaw angle change rate of the leveling robot are obtained. When the acceleration change rate exceeds the second threshold, or the yaw angle change rate exceeds the third threshold, it indicates that the leveling robot is moving or rotating rapidly. At this time, the second pose is used as the state vector of the leveling robot, and the first pose is used as the observation vector of the leveling robot. Adaptive Kalman filtering is applied to the first and second poses to determine the pose of the leveling robot.

[0068] When the rate of change of acceleration does not exceed the second threshold and the rate of change of yaw angle does not exceed the third threshold, it indicates that the leveling machine rotates smoothly without significant shaking, and the camera can obtain an effective recognition image. At this time, the first pose is used as the state vector of the leveling robot, and the second pose is used as the observation vector of the leveling robot. Unscented Kalman filtering is applied to the first pose and the second pose to determine the pose of the leveling machine.

[0069] Understandably, visual information is typically non-linear because cameras measure pixel values ​​in an image, and visual processing involves non-linear operations such as feature extraction and matching. Furthermore, image quality is significantly affected by random noise; factors like changes in lighting and image blur can increase the noise in visual information. Therefore, unscented Kalman filtering is more effective for fusion localization when processing visual information. In contrast, information measured by inertial sensors is typically linear and less affected by random noise. Therefore, adaptive Kalman filtering is more effective for fusion localization when processing information measured by inertial sensors.

[0070] The power infrastructure leveling robot positioning method provided in Example 1, considering the limitations of determining the robot's pose based on image processing and inertial sensors, uses the second pose determined by the inertial sensor (with higher positioning accuracy) as the robot's state vector when the distance between the preset target and the robot exceeds a first threshold. It uses the first pose determined by the camera (with lower positioning accuracy) as the robot's observation vector and employs adaptive Kalman filtering to determine the robot's pose. When the distance between the preset target and the robot does not exceed the first threshold, the method further determines the robot's motion state. During stable motion, the first pose determined by the camera with higher positioning accuracy is used as the state vector of the leveling robot, and the second pose determined by the inertial sensor with lower positioning accuracy is used as the observation vector of the leveling robot. Unscented Kalman filtering is used to determine the pose of the leveling robot. When the leveling robot experiences significant shaking, the second pose determined by the inertial sensor with higher positioning accuracy is used as the state vector of the leveling robot, and the first pose determined by the camera with lower positioning accuracy is used as the observation vector of the leveling robot. Adaptive Kalman filtering is used to determine the pose of the leveling robot, thus avoiding the limitation that accurate positioning can only be achieved when the leveling robot is in a stable motion state.

[0071] Based on the foregoing embodiments, this application provides another preferred embodiment, wherein step S1, acquiring a target image of a preset target based on a camera mounted on the leveling robot, specifically includes:

[0072] S11. Acquire a first image of the site to be leveled based on a camera mounted on the leveling robot.

[0073] The first image is a fixed-size original target image containing a preset target. By processing the original target image, the region of interest (target image) required for subsequent processing can be extracted.

[0074] S12. Perform image filtering on the first image based on a preset filtering method to obtain a first processed image.

[0075] It is understandable that noise will inevitably affect the image acquisition process. In order to improve the accuracy of the pose determination based on the target image, the image needs to be filtered after it is acquired by the camera to remove noise as much as possible for further processing.

[0076] The filtering process of an M×N image through a linear smoothing filter with a convolution kernel of a×b (m, n are odd numbers) can be given by the following formula:

[0077]

[0078] In the formula: w(s,t) represents the weight of the filter kernel at position (s,t); g(x+s,g+t) represents the value of the image at position (x+s,g+t); f(x,y) represents the restored value of the image at (x,y); a and b represent the size of the filter kernel;

[0079] Different filtering effects can be obtained by convolving an image with different filter kernels.

[0080] The following uses different filter kernels to filter the image.

[0081] The operational expression for the mean filter is:

[0082]

[0083] In the formula: S xy This represents the coordinates of a rectangular image window of size m×n centered at (x,y); m, n—represent the size of the filter kernel; g(s,t) represents the value of the image at (s,t); f(x,y) represents the restored value of the image at (x,y) after filtering.

[0084] Mean filters perform local smoothing on images, which effectively reduces noise, but also reduces the sharpness of the processed image.

[0085] Below is the expression for a one-dimensional Gaussian function. In the Gaussian filter kernel, the distance between a pixel and the center pixel is directly proportional to the pixel's weight.

[0086]

[0087] In the formula: σ represents the standard deviation; the size of σ determines the flatness of the function. The larger the σ is, the flatter the function curve, and the smaller the weight of the center pixel will be, resulting in a larger weight of the surrounding pixels, which will make the image smoother.

[0088] The operational expression for the median filter is:

[0089] f(x,y)=median{g(s,t)}

[0090] s,t∈S xy

[0091] Median filtering performs exceptionally well in filtering salt-and-pepper noise. Furthermore, compared to the two aforementioned filters—mean filtering and Gaussian filtering—median filtering better preserves image sharpness and is more suitable for subsequent applications.

[0092] In this embodiment, the first processed image is obtained by using median filtering.

[0093] S13. Perform image enhancement on the first processed image based on a preset enhancement method to obtain a second processed image.

[0094] While filtering can remove noise, it also reduces image clarity, making image features less distinct. Therefore, a preset enhancement method is used to further enhance the first processed image obtained from filtering, making the target points in the enhanced first processed image more prominent and improving the accuracy of subsequent pose determination based on the target image. This preset enhancement method includes, but is not limited to, grayscale transformation (Gamma transformation).

[0095] The expression for grayscale transformation is:

[0096] f(x,y)=T(g(x,y))

[0097] In the formula: g(x,y) represents the gray value of the original image at (x,y); f(x,y) represents the gray value of the transformed image at (x,y); T(·) represents the mapping relationship between the corresponding pixel gray values ​​before and after the transformation;

[0098] Gray-scale transformation can adjust the gray-scale range of an image, thereby improving the image's contrast.

[0099] In this embodiment, the first processed image is subjected to grayscale transformation (Gamma transformation) to obtain the second processed image.

[0100] S14. Perform image segmentation on the second processed image based on a preset segmentation method to obtain a target image of a preset target; wherein, the target image contains a number of target points.

[0101] The first step is to segment and extract the target from the processed image. Based on the discontinuity and similarity of image gray values, an image threshold is set using image similarity. Regions with small gray value differences are regarded as the same object and thus segmented. The image is then converted into a binary image by comparing it with this threshold.

[0102] After thresholding, the binary image will have clear boundaries that can distinguish the feature object from the background. The next step is to perform edge detection on the image. Canny edge detection based on the Sobel operator is used. By fitting the detection with two high and low thresholds, a better contour distribution map can be obtained. Because of noise interference, the low threshold will detect more contours, including many other interfering contours in addition to the important contours. The high threshold will only detect some important contours with higher gradients. Fitting the two results can yield a better result.

[0103] In this embodiment, the enhanced second processed image undergoes thresholding and Canny edge detection to obtain a target image of the preset target.

[0104] Once a target image containing a number of target points is determined, the specific location of the target points can be determined by the centroid method / Hough transform. Then, the center of the feature circle corresponding to each target point is obtained by using the least squares fitting circle method. The center of the circle is used as the target center to determine the position coordinates of the target point in the pixel coordinate system, which prepares for determining the first pose of the leveling robot based on the target image.

[0105] The process of determining the specific location of the target point contour through the centroid method / Hough transform, and then using the least squares fitting circle method to obtain the center of the feature circle corresponding to each target point contour, can be found in the prior art and will not be repeated here.

[0106] In a preferred embodiment, step S2, determining the first pose of the leveling robot based on the target image and the pre-acquired internal parameters of the camera, includes:

[0107] S21. Determine the first coordinates of the target point in the pixel coordinate system.

[0108] Specifically, first, determine the first coordinates of the target point in the target image in the pixel coordinate system. The pixel coordinate system refers to a rectangular coordinate system established with the top-left vertex of the target image as the origin, the shorter side as the x-axis, and the longer side as the y-axis. Distortion caused by lens installation errors (such as the inability to ensure perfect parallelism between the camera sensor and the imaging surface during camera assembly, which introduces tangential distortion) is called tangential distortion. Figure 3 As shown.

[0109] Image distortion caused by the shape of the lens itself and installation errors can significantly reduce the accuracy of visual positioning. Therefore, the influence of distortion cannot be ignored during pose determination. Radial distortion increases with the distance to the optical center, and the effect of distortion can be characterized by a distance polynomial function:

[0110] x corrected=x(1+k1r) 2 +k2r 4 +k3r 6 )

[0111] y corrected =y(1+k1r 2 +k2r 4 +k3r 6 )

[0112] In the formula: [x,y] T Represents the coordinates of the uncorrected image point; [x corrected ,y corrected ] T The values ​​represent the corrected image point coordinates; k1, k2, and k3 represent the distortion parameters.

[0113] For ordinary cameras, the parameters k1 and k2 are sufficient to characterize the impact of radial distortion. However, for some special lenses such as fisheye cameras, the k3 term is needed to correct distortion. The formula for correcting tangential distortion is:

[0114] x corrected =x + 2p1xy + p2(r 2 +2x 2 )

[0115] y corrected =y+2p2xy+p1(r 2 +2y 2 )

[0116] By performing distortion correction (barrel distortion and pincushion distortion) on the target image, the true pixel coordinates of the feature points on the image can be obtained.

[0117] S22. Determine the second coordinates of the target point in the world coordinate system.

[0118] Specifically, the second coordinates of the target point are determined in the world coordinate system, and the target point corresponds to the target point determined in the primitive coordinate system. The world coordinate system refers to a three-dimensional Cartesian coordinate system established with the centroid of the leveling robot as the origin and the x, y, and z axes pointing towards the lower right of the leveling robot.

[0119] S23. Based on the pre-acquired internal parameters of the camera, the first coordinate and the second coordinate, determine the first pose of the leveling robot using bundle adjustment.

[0120] It is understandable that, based on the aforementioned target image, the target code currently being identified can be determined, and thus the position of the preset target in the world coordinate system can be known. Combined with the first pose of the leveling robot in the world coordinate system, the distance between the preset target and the leveling robot can be calculated.

[0121] It is important to note that as the distance between the camera and the preset target increases, the obtained feature points are more severely affected by noise, which may lead to mismatches. In this case, the position determined by the PNP algorithm may have a large error. Therefore, this embodiment uses bundle adjustment to optimize the camera pose iteratively, minimizing the impact of noise.

[0122] We conducted a comparative experiment, studying how to determine the first pose of a leveling robot using the PNP algorithm and bundle adjustment at different distances. In the experiment, the distance between the camera and the pre-set target was divided into three intervals: relatively close, medium, and relatively far. We found that, under the same pose, the pose data obtained by bundle adjustment was more stable than that obtained by the PNP algorithm. However, as the distance between the camera and the pre-set target increased, the variance of the data calculated by the PNP algorithm also gradually increased. At long distances, fluctuations even exceeded meters. Therefore, we can conclude that bundle adjustment is more suitable than the PNP algorithm at relatively close distances, while the PNP algorithm is more suitable at relatively long distances. These findings are of great significance for robot localization technology. Bundle adjustment does not exhibit the drastic fluctuations seen in the PNP algorithm; therefore, this embodiment uses bundle adjustment to determine the first pose of the leveling robot. Bundle adjustment is described in existing technologies and will not be elaborated upon here.

[0123] The power infrastructure leveling robot positioning method provided by the above-mentioned technical solution of this application has the following advantages: Considering the limitations of determining the pose of the leveling robot based on image processing and based on inertial sensors, when the distance between the preset target and the leveling robot exceeds a first threshold, the second pose determined by the inertial sensor with higher positioning accuracy is used as the state vector of the leveling robot, and the first pose determined by the camera with lower positioning accuracy is used as the observation vector of the leveling robot. Adaptive Kalman filtering is used to determine the pose of the leveling robot; when the distance between the preset target and the leveling robot does not exceed the first threshold, the motion state of the leveling robot is further judged. When the leveling robot moves smoothly, the first pose determined by the camera with high positioning accuracy is used as the state vector of the leveling robot, and the second pose determined by the inertial sensor with lower positioning accuracy is used as the observation vector of the leveling robot. Unscented Kalman filtering is used to determine the pose of the leveling robot. When the leveling robot experiences significant shaking, the second pose determined by the inertial sensor with high positioning accuracy is used as the state vector of the leveling robot, and the first pose determined by the camera with lower positioning accuracy is used as the observation vector of the leveling robot. Adaptive Kalman filtering is used to determine the pose of the leveling robot. This avoids the limitation that accurate positioning can only be achieved when the leveling robot is in a stable motion state.

[0124] Embodiment 2 of this application provides a positioning system for a power infrastructure leveling robot. In Embodiment 2, the system includes:

[0125] The image acquisition module is used to acquire target images of preset targets through a camera set on the leveling robot;

[0126] The first calculation module is used to determine the first pose of the leveling robot based on the target image and the pre-acquired internal parameters of the camera; and to determine the distance between the camera and the preset target based on the first pose.

[0127] The second calculation module is used to determine the second pose of the leveling robot by means of an inertial sensor mounted on the leveling robot.

[0128] The positioning module is used to determine the pose of the leveling robot based on the distance, the first pose, and the second pose; specifically, the positioning module is used for:

[0129] When the distance exceeds the first threshold, the second pose is used as the state vector of the leveling robot, the first pose is used as the observation vector of the leveling robot, and an adaptive Kalman filter is applied to the first pose and the second pose to obtain the pose of the leveling robot.

[0130] When the distance does not exceed a first threshold time, the acceleration change rate and yaw angle change rate of the leveling robot are obtained based on the inertial sensor. When the acceleration change rate exceeds a second threshold or the yaw angle change rate exceeds a third threshold, the second pose is used as the state vector of the leveling robot, and the first pose is used as the observation vector of the leveling robot. An adaptive Kalman filter is applied to the first pose and the second pose to obtain the pose of the leveling robot. When the acceleration change rate does not exceed the second threshold and the yaw angle change rate does not exceed the third threshold, the first pose is used as the state vector of the leveling robot, and the second pose is used as the observation vector of the leveling robot. An unscented Kalman filter is applied to the first pose and the second pose to obtain the pose of the leveling robot.

[0131] Preferably, the image acquisition module is specifically used for:

[0132] A first image of the site to be leveled is acquired using a camera mounted on the leveling robot;

[0133] The first image is filtered based on a preset filtering method to obtain a first processed image;

[0134] The first processed image is enhanced based on a preset enhancement method to obtain a second processed image;

[0135] The second processed image is segmented based on a preset segmentation method to obtain a target image of a preset target; wherein the target image contains a number of target points.

[0136] Embodiment 3 of this application provides a positioning device for a power infrastructure leveling robot, including: a memory and a processor;

[0137] The memory is used to store computer programs;

[0138] The processor is used to implement, when executing the computer program, a power infrastructure leveling robot positioning method as described in Embodiment 1 or 2.

[0139] Those skilled in the art will clearly understand that, for the sake of convenience and brevity, the specific working processes of the systems, devices, and units described above can be referred to the corresponding processes in the foregoing method embodiments, and will not be repeated here.

[0140] In the several embodiments provided in this application, it should be understood that the disclosed systems, apparatuses, and methods can be implemented in other ways. For example, the apparatus embodiments described above are merely illustrative; for instance, the division of units is only a logical functional division, and in actual implementation, there may be other division methods. For example, multiple units or components may be combined or integrated into another system, or some features may be ignored or not executed. Furthermore, the coupling or direct coupling or communication connection shown or discussed may be an indirect coupling or communication connection between apparatuses or units through some interfaces, and may be electrical, mechanical, or other forms.

[0141] The units described as separate components may or may not be physically separate. The components shown as units may or may not be physical units; they may be located in one place or distributed across multiple network units. Some or all of the units can be selected to achieve the purpose of this embodiment, depending on actual needs.

[0142] Furthermore, the functional units in the various embodiments of the present invention can be integrated into one processing unit, or each unit can exist physically separately, or two or more units can be integrated into one unit. The integrated unit can be implemented in hardware or as a software functional unit.

[0143] If the integrated unit is implemented as a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present invention, in essence, or the part that contributes to the prior art, or all or part of the technical solution, can be embodied in the form of a software product. This computer software product is stored in a storage medium and includes several instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) to execute all or part of the steps of the methods described in the various embodiments of the present invention. The aforementioned storage medium includes various media capable of storing program code, such as USB flash drives, portable hard drives, read-only memory (ROM), random access memory (RAM), magnetic disks, or optical disks.

[0144] The above-described embodiments are only used to illustrate the technical solutions of the present invention, and are not intended to limit it. Although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some of the technical features. Such modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. A positioning method for a power infrastructure leveling robot, characterized in that, include: The target image of the preset target is obtained by a camera set on the leveling robot; Based on the target image and the pre-acquired internal parameters of the camera, the first pose of the leveling robot is determined; the distance between the camera and the preset target is determined based on the first pose. The second pose of the leveling robot is determined by an inertial sensor mounted on the robot. Determining the pose of the leveling robot based on the distance, the first pose, and the second pose includes: When the distance exceeds the first threshold, the second pose is used as the state vector of the leveling robot, the first pose is used as the observation vector of the leveling robot, and an adaptive Kalman filter is applied to the first pose and the second pose to obtain the pose of the leveling robot. When the distance does not exceed a first threshold time, the acceleration change rate and yaw angle change rate of the leveling robot are obtained based on the inertial sensor. When the acceleration change rate exceeds a second threshold or the yaw angle change rate exceeds a third threshold, the second pose is used as the state vector of the leveling robot, and the first pose is used as the observation vector of the leveling robot. An adaptive Kalman filter is applied to the first pose and the second pose to obtain the pose of the leveling robot. When the acceleration change rate does not exceed the second threshold and the yaw angle change rate does not exceed the third threshold, the first pose is used as the state vector of the leveling robot, and the second pose is used as the observation vector of the leveling robot. An unscented Kalman filter is applied to the first pose and the second pose to obtain the pose of the leveling robot.

2. The positioning method for a power infrastructure leveling robot according to claim 1, characterized in that, The step of acquiring a target image of a preset target using a camera mounted on the leveling robot includes: A first image of the site to be leveled is acquired using a camera mounted on the leveling robot; The first image is filtered based on a preset filtering method to obtain a first processed image; The first processed image is enhanced based on a preset enhancement method to obtain a second processed image; The second processed image is segmented based on a preset segmentation method to obtain a target image of a preset target; wherein the target image contains a number of target points.

3. The positioning method for a power infrastructure leveling robot according to claim 2, characterized in that, Determining the first pose of the leveling robot based on the target image and the pre-acquired internal parameters of the camera includes: Determine the first coordinates of the target point in the pixel coordinate system; Determine the second coordinates of the target point in the world coordinate system; Based on the pre-acquired internal parameters of the camera, the first coordinate, and the second coordinate, the first pose of the leveling robot is determined using bundle adjustment.

4. The positioning method for a power infrastructure leveling robot according to claim 3, characterized in that, The step of segmenting the second processed image based on a preset segmentation method to obtain a target image of a preset target includes: The second processed image is subjected to thresholding and Canny edge detection to obtain a target image of the preset target.

5. The positioning method for a power infrastructure leveling robot according to claim 4, characterized in that, Determining the first coordinates of the target point in the pixel coordinate system includes: The target center of the target point is determined based on the centroid method and the least squares fitted circle method, and the position of the target center in the pixel coordinate system is recorded as the first coordinate of the target point.

6. The positioning method for a power infrastructure leveling robot according to claim 2, characterized in that, The preset filtering method is mean filtering, Gaussian filtering, or median filtering.

7. The positioning method for a power infrastructure leveling robot according to claim 2, characterized in that, The preset enhancement method is grayscale transformation.

8. A positioning system for a power infrastructure leveling robot, characterized in that, include: The image acquisition module is used to acquire target images of preset targets through a camera set on the leveling robot; The first calculation module is used to determine the first pose of the leveling robot based on the target image and the pre-acquired internal parameters of the camera; and to determine the distance between the camera and the preset target based on the first pose. The second calculation module is used to determine the second pose of the leveling robot by means of an inertial sensor mounted on the leveling robot. The positioning module is used to determine the pose of the leveling robot based on the distance, the first pose, and the second pose; specifically, the positioning module is used for: When the distance exceeds the first threshold, the second pose is used as the state vector of the leveling robot, the first pose is used as the observation vector of the leveling robot, and an adaptive Kalman filter is applied to the first pose and the second pose to obtain the pose of the leveling robot. When the distance does not exceed a first threshold time, the acceleration change rate and yaw angle change rate of the leveling robot are obtained based on the inertial sensor. When the acceleration change rate exceeds a second threshold or the yaw angle change rate exceeds a third threshold, the second pose is used as the state vector of the leveling robot, and the first pose is used as the observation vector of the leveling robot. An adaptive Kalman filter is applied to the first pose and the second pose to obtain the pose of the leveling robot. When the acceleration change rate does not exceed the second threshold and the yaw angle change rate does not exceed the third threshold, the first pose is used as the state vector of the leveling robot, and the second pose is used as the observation vector of the leveling robot. An unscented Kalman filter is applied to the first pose and the second pose to obtain the pose of the leveling robot.

9. A power infrastructure leveling robot positioning system according to claim 8, characterized in that, The image acquisition module is specifically used for: A first image of the site to be leveled is acquired using a camera mounted on the leveling robot; The first image is filtered based on a preset filtering method to obtain a first processed image; The first processed image is enhanced based on a preset enhancement method to obtain a second processed image; The second processed image is segmented based on a preset segmentation method to obtain a target image of a preset target; wherein the target image contains a number of target points.

10. A positioning device for a power infrastructure leveling robot, characterized in that, include: Memory and processor; The memory is used to store computer programs; The processor is configured to implement, when executing the computer program, a power infrastructure leveling robot positioning method as described in any one of claims 1 to 7.

Images