A method for autonomous and precise battery replacement of UAVs based on visual servoing and collaborative positioning

Abstract

The present application relates to the field of drone control technology, and in particular to a method for autonomous and precise battery replacement of drones based on visual servoing and collaborative positioning. The method includes: acquiring image data through a fixed camera and a terminal camera; extracting feature points in the image data, matching the feature points to detect target clusters, estimating the preliminary pose based on the target clusters and the perspective n-point algorithm; calculating the confidence weight of each camera; fusing the preliminary poses of the fixed camera and the terminal camera in the three-dimensional rigid body transformation space to obtain a fused pose; calculating the position error and attitude error based on the difference between the fused pose and the current pose of the robotic arm, generating control instructions for the robotic arm, driving the robotic arm to perform battery replacement operations, and performing a retry operation when a battery replacement failure is detected. The environmental adaptability and operational accuracy of the autonomous battery replacement system of drones are improved, and the limitations of traditional single-camera systems under occlusion, lighting and perspective interference are effectively overcome through multi-perspective collaborative perception and dynamic optimization mechanisms.

Description

Technical Field

[0001] The present application relates to the field of drone control technology, and in particular to a method for autonomous and precise battery replacement of drones based on visual servoing and collaborative positioning. Background Art

[0002] With the widespread adoption of drones in areas such as industrial inspection and logistics, their continued operational capabilities are highly dependent on fast and accurate autonomous battery replacement technology. Current mainstream solutions utilize positioning methods based on visual markers, with the AprilTag QR code target system becoming a popular choice due to its high recognition rate and sub-pixel positioning accuracy. This technology pre-places a QR code target with a specific code. After capturing the target image with a camera, it calculates the target's pose relative to the camera through corner detection and perspective transformation.

[0003] However, in actual deployment, such systems generally rely on a single visual sensor architecture, that is, pose estimation is achieved only through a fixed global camera or a single terminal camera onboard the drone. This single-view perception mode has fundamental limitations: when the target is partially occluded, subject to extreme lighting interference, or the camera's perspective deviates, the integrity of image feature extraction will be significantly reduced, causing the pose solution error to increase to the centimeter level. Especially for mechanical operations with millimeter-level precision such as battery insertion and removal, existing technologies are prone to positioning drift and even recognition failure due to insufficient environmental adaptability, forcing the system to frequently interrupt operations and rely on manual intervention. More critically, traditional solutions lack an active evaluation mechanism for image quality degradation (such as motion blur and uneven brightness), and are unable to dynamically adjust perception strategies under complex working conditions. This has become a core bottleneck restricting the large-scale implementation of fully autonomous battery swapping technology for drones. Summary of the Invention

[0004] In order to solve the technical problems existing in the prior art, the present application provides a method for autonomous and precise battery replacement of a UAV based on visual servoing and collaborative positioning, comprising the following steps:

[0005] The fixed camera acquires image data for overall viewing angle observation, and the end camera at the end of the robotic arm acquires image data for target identification;

[0006] Extract feature points from the image data and match the feature points with a preset target database to detect target clusters. n Point algorithm estimates preliminary pose;

[0007] Calculating a confidence weight for each camera, the confidence weight being dynamically determined based on image quality metrics including the number of feature points, image blur, and brightness uniformity;

[0008] In a three-dimensional rigid body transformation space, the preliminary poses of the fixed camera and the end camera are fused using a Lie algebra optimization method, and combined with the confidence weight to obtain a fused pose;

[0009] Based on the difference between the fused posture and the current posture of the robotic arm, the position error and posture error are calculated, and a control instruction for the robotic arm is generated to drive the robotic arm to perform a battery replacement operation and perform a retry operation when a battery replacement failure is detected.

[0010] Specifically, the target cluster consists of multiple AprilTag QR codes, and the relative position of each QR code in the world coordinate system is fixed;

[0011] The method for extracting feature points from the image data is:

[0012] The corner points of the QR code are identified by an image processing module and matched with the preset geometric layout in the target database.

[0013] Specifically, the target cluster and perspective n The point algorithm estimates the initial pose by minimizing the reprojection error function, which is:

[0014] ,

[0015] in, represents the camera projection function, and are the rotation matrix and translation vector respectively, is the target point in the world coordinate system, is the detected pixel point, is the number of target points.

[0016] Specifically, the method for calculating the confidence weight of each camera is completed by a confidence weight calculation formula, and the confidence weight calculation formula is:

[0017] ,

[0018] in is the number of feature points in the current image, is the fuzziness index, ∈[0,1], is the brightness standard deviation, M is the number of cameras. The weight calculation module evaluates the image quality of each camera in real time and dynamically adjusts the fusion weight.

[0019] Specifically, the method of fusing the preliminary poses of the fixed camera and the terminal camera using the Lie algebra optimization method is completed by a fusion formula, and the fusion formula is:

[0020] ,

[0021] in,( , ) represents the initial pose obtained by the fixed camera, ( , ) represents the initial pose obtained by the end camera, are the confidence weights of the fixed camera and the terminal camera respectively, is a Lie algebra mapping, and are the rotation matrix and translation vector respectively, represents the fused pose.

[0022] Specifically, the method for generating a control instruction for a robotic arm includes:

[0023] According to the fusion pose and the current posture of the end of the robotic arm The difference between the two is used to calculate the position error. and attitude error , the calculation formula is:

[0024] ,

[0025] Generate the control instructions of the robotic arm, the generation formula is:

[0026] + ,

[0027] in, To control the movement vector of the terminal velocity, and are the control gains of position and attitude.

[0028] Specifically, the method further includes:

[0029] When the image quality index is lower than the quality threshold, the preliminary pose is subjected to multi-view weighted minimization fusion to obtain the optimal fused pose. The formula for the multi-view weighted minimization fusion is:

[0030] ,

[0031] in, is the optimal fusion pose, T is the pose transformation matrix, is the jth feature point in the world coordinate system, represents the projection function of the i-th camera model, is the pixel position detected at the i-th viewing angle, The confidence weight of the i-th view.

[0032] Specifically, performing a retry operation when a battery replacement failure is detected includes:

[0033] The battery swapping status is detected based on the force or torque sensor or visual status at the end of the robotic arm. When the battery swapping fails, a retry operation is performed, which includes safely retracting the robotic arm, recalibrating the target cluster in a small range, and re-inserting and unplugging.

[0034] During the retry process, image data is reacquired and the fusion pose is updated until the battery swap is successful or the maximum number of retries is reached.

[0035] Specifically, among the image quality indicators:

[0036] The image blur is obtained by calculating the image gradient amplitude or frequency domain analysis;

[0037] The brightness uniformity is obtained by calculating and evaluating the standard deviation of the image brightness;

[0038] The number of feature points is determined based on the AprilTag matching score.

[0039] Specifically, the fixed camera is arranged on the top of the hangar, and the terminal camera is integrated with a six-dimensional force sensor. When performing the battery replacement operation, the fixed camera globally observes the drone's posture, and the terminal camera activates high-precision recognition when the distance from the battery interface is <10 cm.

[0040] This application has the following technical effects:

[0041] The environmental adaptability and operational accuracy of the drone's autonomous battery swapping system have been improved. Through multi-perspective collaborative perception and dynamic optimization mechanisms, the limitations of traditional single-camera systems under occlusion, lighting, and perspective interference have been effectively overcome.

[0042] The system fuses heterogeneous visual data from fixed cameras and terminal cameras, and uses image quality-driven confidence assessment to adjust the pose fusion strategy in real time, improving the battery docking pose estimation accuracy from centimeters to millimeters.

[0043] At the same time, the Lie algebra space optimization method ensures the stability of attitude solution under complex interference. Combined with closed-loop servo control and autonomous retry mechanism, it can achieve high-reliability battery replacement operations in industrial scenarios such as strong light obstruction, completely getting rid of dependence on human intervention. BRIEF DESCRIPTION OF THE DRAWINGS

[0044] By reading the detailed description below with reference to the accompanying drawings, the above and other objects, features and advantages of the exemplary embodiments of the present application will become readily understood. In the accompanying drawings, several embodiments of the present application are shown in an exemplary and non-limiting manner, and the same or corresponding numbers represent the same or corresponding parts.

[0045] Figure 1 This is a flowchart of the steps of the method for autonomous and precise battery replacement of a drone based on visual servoing and collaborative positioning in an embodiment of the present application. DETAILED DESCRIPTION

[0046] The technical solutions in the embodiments of the present application will be described clearly and completely below in conjunction with the drawings in the embodiments of the present application.

[0047] The large-scale deployment of drones in industrial scenarios such as power inspection and warehousing and logistics urgently requires a sustainable energy supply. However, existing autonomous battery swapping technologies are limited by insufficient visual positioning accuracy, making them incapable of meeting millimeter-level mechanical docking requirements. Traditional solutions generally employ monocular visual positioning based on the AprilTag QR code target. This technology estimates the target's pose by identifying corner points of a pre-coded QR code and calculating the perspective transformation matrix. The AprilTag system inherently boasts sub-pixel corner detection accuracy, theoretically enabling high-precision positioning. However, the complex industrial environment fully exposes the inherent limitations of this single-camera architecture. When a drone enters a hangar, the fixed camera's global view is susceptible to target obstruction by the aircraft arm, resulting in missing feature points. Strong backlighting or uneven shadows can cause over- or underexposure in the image, reducing recognition confidence. Furthermore, when the target plane's angle with the optical axis exceeds 60 degrees in a single view, perspective distortion is amplified. Furthermore, dynamic interference such as motion blur and mechanical vibration cannot be compensated for through static calibration. These factors combine to result in actual pose estimation errors of three to five centimeters, far exceeding the physical tolerance of one to two millimeters for the battery plug-in interface. Existing technologies attempt to alleviate this problem by increasing the number of targets or introducing redundant cameras, but lack an intelligent fusion mechanism for multi-source data. The fixed-weight fusion mode introduces noise when the local perspective fails, and ultimately still relies on preset trajectories and manual calibration, which seriously restricts the robustness of the system in real scenarios.

[0048] Based on this, this embodiment proposes a method for autonomous and precise battery replacement of UAVs based on visual servoing and collaborative positioning. Figure 1 As shown, the following steps are included:

[0049] The fixed camera is used to obtain image data for overall perspective observation, and the end camera installed at the end of the UAV's robotic arm is used to obtain image data for target identification;

[0050] Extract feature points from image data and match them with the preset target database to detect target clusters. nPoint algorithm estimates preliminary pose;

[0051] Calculate the confidence weight for each camera. The confidence weight is dynamically determined based on image quality indicators, such as the number of feature points, image blur, and brightness uniformity.

[0052] In the 3D rigid body transformation space, the Lie algebra optimization method is used to fuse the preliminary poses of the fixed camera and the end camera, and the fused pose is obtained by combining the confidence weights.

[0053] Based on the difference between the fused posture and the current posture of the robotic arm, the position error and posture error are calculated, and the control instructions of the robotic arm are generated to drive the robotic arm to perform the battery replacement operation and perform a retry operation when the battery replacement failure is detected.

[0054] In this embodiment, the system hardware deployment includes three key nodes: two global observation high-definition cameras are installed on the top of the hangar to cover the drone's entry path, a miniature vision module with a six-dimensional force sensor is integrated at the end of the six-axis collaborative robotic arm, and a target cluster consisting of four AprilTags is arranged on the surface of the battery compartment, and the world coordinates of each target are pre-calibrated. When the drone lands at the switching point, the global camera captures the coarse positioning image of the body at a frequency of fifteen Hz, and the end camera activates a high frame rate recognition of one hundred Hz when the robotic arm is within ten centimeters of the battery compartment. The raw images of each camera are input into the parallel processing pipeline, and the AprilTag is first identified through adaptive threshold segmentation and sub-pixel corner extraction. A set of three-dimensional to two-dimensional point correspondences is generated for each detected target. Specifically, the method for extracting feature points in image data is as follows:

[0055] The QR code corner points are identified through the image processing module and matched with the preset geometric layout in the target database.

[0056] At this time, the traditional solution is to use perspective n The point algorithm solves the pose of a single camera, while this embodiment adds an image quality assessment layer to dynamically adjust the data credibility. Specifically, based on the target cluster and perspective n The point algorithm estimates the initial pose by minimizing the reprojection error function, which is:

[0057] ,

[0058] in, represents the camera projection function, and are the rotation matrix and translation vector respectively, is the target point in the world coordinate system, is the detected pixel point, is the number of target points.

[0059] In this embodiment, the confidence weight of each camera is calculated using a confidence weight calculation formula, which is:

[0060] ,

[0061] in is the number of feature points in the current image, is the fuzziness index, ∈[0,1], is the brightness standard deviation, M is the number of cameras. The weight calculation module evaluates the image quality of each camera in real time and dynamically adjusts the fusion weights. These three metrics have clear physical meanings: the number of feature points directly reflects the degree of occlusion, the blur metric quantifies the impact of motion jitter, and the brightness standard deviation represents the intensity of illumination interference. Image blur is obtained by calculating the image gradient amplitude or frequency domain analysis; brightness uniformity is determined by calculating and evaluating the standard deviation of image brightness. The number of feature points is determined based on the AprilTag matching score. Specifically, the system generates an initial matching confidence score (0-100) by decoding the binary information of the QR code. This score is then corrected by combining corner detection quality and geometric consistency verification. First, the extracted corner points of the QR code boundary are located, and the root mean square error (RMS) between the actual corner points and the ideal grid position is calculated as the corner quality coefficient. Second, the perspective transformation model is used to reproject the target's 3D coordinates onto the image plane, and the inverse of the reprojected pixel error is used as the geometric consistency coefficient. The final matching score is the normalized product of the decoding confidence, corner quality coefficient, and geometric consistency coefficient, which directly reflects the reliability and completeness of the current target feature points. The confidence weight of the current viewpoint is calculated based on real-time metrics using a formula. This weight determines the influence of each camera in the subsequent fusion process and is mathematically expressed as a normalized weighted value. If the global camera's brightness standard deviation increases significantly due to strong backlight, its weight is automatically reduced to prevent poor quality data from contaminating the fusion result.

[0062] The pose fusion process completely abandons the Euler angle or quaternion representation and instead adopts a unified modeling method in Lie group space. The preliminary pose of each camera solved by minimizing the reprojection error is converted into a special Euclidean group element, and the fusion process is transformed into a weighted optimization problem on the Lie algebra manifold. Specifically, the method of using Lie algebra optimization method to fuse the preliminary poses of the fixed camera and the end camera is completed through the fusion formula, which is:

[0063] ,

[0064] in,( , ) represents the initial pose obtained by fixing the camera, ( , ) represents the initial pose obtained by the end camera, are the confidence weights of the fixed camera and the end camera respectively, is a Lie algebra mapping, and are the rotation matrix and translation vector respectively, represents the fused pose. This mathematical architecture fundamentally avoids the Euler angle gimbal lock problem, while a dynamic weighting mechanism ensures a stable output even when partial view angles fail. Actual measurement data shows that even with strong interference from a single camera with a view angle ambiguity of 0.5, the standard deviation of the position error of the fused pose is kept within 0.8 mm, and the angular error is less than 0.3 degrees, achieving a fourfold improvement in accuracy compared to traditional monocular vision.

[0065] The difference between the fused pose and the current pose of the robotic arm drives the visual servo controller to generate real-time motion commands. Position error is directly expressed as a three-dimensional spatial displacement vector, while the pose error is converted into a rotation vector through Lie algebraic mapping. Both are multiplied by the proportional gain coefficient and superimposed to form a six-dimensional velocity control variable. This control variable is input into the robotic arm trajectory planner, and closed-loop correction is completed within a 20-millisecond cycle. Specifically, the method for generating control commands for the robotic arm includes:

[0066] According to the fusion pose and the current posture of the end of the robotic arm The difference between the two is used to calculate the position error. and attitude error , the calculation formula is:

[0067] ,

[0068] Generate the control instructions of the robotic arm, the generation formula is:

[0069] + ,

[0070] in, To control the movement vector of the terminal velocity, and are the control gains of position and attitude.

[0071] The core of improving the robustness of the system lies in the multi-level anti-interference design. At the perception layer, the dynamic weight mechanism enables the system to rely on the terminal near-field vision to maintain operation when the global camera fails. It is measured that when 50% of the target is blocked, the positioning accuracy is only attenuated by 15%. At the algorithm layer, Lie algebra optimization ensures the mathematical completeness of the attitude estimation, and the attitude solution error at the extreme pitch angle is reduced by 60 times compared with the quaternion interpolation method. At the execution layer, the retry strategy based on force feedback builds the last line of defense to eliminate mechanical jams caused by cumulative errors. Specifically, the method in this embodiment also includes:

[0072] When the image quality index is lower than the quality threshold, the multi-view weighted minimization fusion is used for the preliminary pose to obtain the optimal fusion pose. The formula for multi-view weighted minimization fusion is:

[0073] ,

[0074] in, is the optimal fusion pose, T is the pose transformation matrix, is the jth feature point in the world coordinate system, represents the projection function of the i-th camera model, is the pixel position detected at the i-th viewing angle, The confidence weight of the i-th view.

[0075] The last step is to perform a retry operation when a battery swap failure is detected, including:

[0076] The battery swap status is detected based on the force or torque sensor or visual status at the end of the robotic arm. When the battery swap fails, a retry operation is performed. The retry operation includes safely retracting the robotic arm, recalibrating the target cluster in a small range, and re-inserting and unplugging.

[0077] During the retry process, image data is reacquired and the fusion pose is updated until the battery swap is successful or the maximum number of retries is reached.

[0078] In this embodiment, a fixed camera is installed on the top of the hangar, and the terminal camera is integrated with a six-dimensional force sensor. When performing the battery replacement operation, the fixed camera globally observes the drone's posture, and the terminal camera activates high-precision recognition when the distance from the battery interface is <10 cm.

[0079] Obviously, the embodiments described above are only part of the embodiments of this application, not all of them. Based on the embodiments in this application, all other embodiments obtained by those skilled in the art without creative work are within the scope of protection of this application.

[0080] It should be understood that when the terms "first," "second," etc. are used in the claims, specification, and drawings of this application, they are only used to distinguish different objects, rather than to describe a specific order. The terms "comprise" and "comprising" used in the specification and claims of this application indicate the presence of the described features, integers, steps, operations, elements, and / or components, but do not exclude the presence or addition of one or more other features, integers, steps, operations, elements, components, and / or combinations thereof.

Claims

1. A method for autonomous and precise battery replacement of UAV based on visual servoing and collaborative positioning, characterized in that: The following steps are involved: The fixed camera acquires image data for overall viewing angle observation, and the end camera at the end of the robotic arm acquires image data for target identification; Extract the feature points in the image data and match the feature points with the preset target database to detect the target cluster. n The point algorithm estimates the preliminary pose; Calculating a confidence weight for each camera, the confidence weight being dynamically determined based on image quality metrics including the number of feature points, image blur, and brightness uniformity; In a three-dimensional rigid body transformation space, the preliminary poses of the fixed camera and the end camera are fused using a Lie algebra optimization method, and combined with the confidence weight to obtain a fused pose; Based on the difference between the fused posture and the current posture of the robotic arm, the position error and the posture error are calculated, and a control instruction for the robotic arm is generated to drive the robotic arm to perform a battery replacement operation and perform a retry operation when a battery replacement failure is detected; The target cluster and perspective n The point algorithm estimates the initial pose by minimizing the reprojection error function, which is: ， in, represents the camera projection function, and are the rotation matrix and translation vector respectively, is the target point in the world coordinate system, is the detected pixel point, is the number of target points; The method of fusing the preliminary poses of the fixed camera and the terminal camera using the Lie algebra optimization method is completed by a fusion formula, which is: ， in,( , ) represents the initial pose obtained by the fixed camera, ( , ) represents the initial pose obtained by the end camera, are the confidence weights of the fixed camera and the terminal camera respectively, is a Lie algebra mapping, and are the rotation matrix and translation vector respectively, represents the fusion pose; The method for generating a control instruction for a robotic arm comprises: According to the fusion pose and the current posture of the end of the robotic arm The difference between the two is used to calculate the position error. and attitude error , the calculation formula is: ， Generate the control instructions of the robotic arm, the generation formula is: + ， in, To control the movement vector of the terminal velocity, and are the control gains of position and attitude.

2. The method according to claim 1, characterized in that The target cluster consists of multiple AprilTag QR codes, and the relative position of each QR code in the world coordinate system is fixed; The method for extracting feature points from the image data is: The corner points of the QR code are identified by an image processing module and matched with the preset geometric layout in the target database.

3. The method according to claim 1, characterized in that The method for calculating the confidence weight of each camera is completed by a confidence weight calculation formula, and the confidence weight calculation formula is: ， in is the number of feature points in the current image, is the fuzziness index, ∈[0,1], is the brightness standard deviation, M is the number of cameras. The weight calculation module evaluates the image quality of each camera in real time and dynamically adjusts the fusion weight.

4. The method according to claim 1, wherein The method further comprises: When the image quality index is lower than the quality threshold, the preliminary pose is subjected to multi-view weighted minimization fusion to obtain the optimal fused pose. The formula for the multi-view weighted minimization fusion is: ， in, is the optimal fusion pose, T is the pose transformation matrix, is the jth feature point in the world coordinate system, represents the projection function of the i-th camera model, is the pixel position detected at the i-th viewing angle, The confidence weight of the i-th view.

5. The method according to claim 1, wherein The retry operation when detecting battery replacement failure includes: The battery swapping status is detected based on the force or torque sensor or visual status at the end of the robotic arm. When the battery swapping fails, a retry operation is performed, which includes safely retracting the robotic arm, recalibrating the target cluster in a small range, and re-inserting and unplugging. During the retry process, image data is reacquired and the fusion pose is updated until the battery swap is successful or the maximum number of retries is reached.

6. The method according to claim 1, characterized in that Among the image quality indicators: The image blur is obtained by calculating the image gradient amplitude or frequency domain analysis; The brightness uniformity is obtained by calculating and evaluating the standard deviation of the image brightness; The number of feature points is determined based on the AprilTag matching score.

7. The method according to claim 1, characterized in that The fixed camera is arranged on the top of the hangar, and the terminal camera is integrated with a six-dimensional force sensor. When performing the battery replacement operation, the fixed camera globally observes the drone's posture, and the terminal camera activates high-precision recognition when the distance from the battery interface is <10 cm.

Images