Pose estimation method and system, and guidance method and system based on ROS2

Abstract

The invention discloses an ROS2-based pose estimation method and system, and a guidance method and system, belongs to the technical field of intelligent robots and automatic control, and solves the problems that in the prior art, the three-dimensional pose and the physical volume of a battery cannot be sensed, physical marks are depended, the recognition rate is low in outdoor strong light and wear environments, and the reliability is high. Therefore, the technical problems that the four-degree-of-freedom mechanical arm is high in grabbing failure rate and poor in safety under the non-planar working condition, and an existing model is high in calculation complexity, large in memory occupation and remarkable in system processing delay are solved. Obtaining and synchronizing a depth image and an RGB image; carrying out identification by using a target detection model, screening an optimal bounding box of which the category is a battery, and obtaining a depth sub-matrix of the ROI region; and performing double filtering on the depth sub-matrix to obtain a smooth depth map, and calculating a projection angle of a unit normal vector of the battery surface in an X-Z plane based on the unit normal vector of the battery surface to obtain a battery rotation angle. The method is used for realizing safe and efficient unmanned aerial vehicle automatic battery replacement.

Description

ROS2-based pose estimation methods and systems, guidance methods and systems Technical Field

[0001] This invention relates to the field of intelligent robot and automation control technology, specifically to a pose estimation method and system based on ROS2, and a guidance method and system. Background Technology

[0002] Fully automated battery swapping platforms are core equipment for solving the endurance bottleneck of industrial-grade drones. The key lies in the high-precision, real-time guidance of the drone and battery by a vision system. However, existing technologies mainly rely on manual marking and localization based on the ROS1 (Robot Operating System 1) architecture or traditional edge detection algorithms, which have significant drawbacks in real-world outdoor scenarios: manual marking schemes are easily rendered ineffective by wear and tear, oil contamination, or lighting interference caused by high-frequency operations, and cannot perceive the physical volume of the battery to avoid collisions; traditional ROS1-based systems suffer from high latency when transmitting high-resolution images; and traditional vision algorithms struggle to accurately segment target contours when faced with complex cabin backgrounds and drone attitude deviations. These shortcomings in environmental adaptability and anti-interference capabilities severely restrict the success rate and safety of battery swapping operations.

[0003] In the prior art, Chinese patent document CN115861780A discloses "A robotic arm detection and grasping method based on YOLO-GGCNN". This method utilizes a YOLOv4 deep learning network to train the target to be grasped, obtaining a trained model. Before the robotic arm grasps, a depth camera is used to acquire a blank depth image of the grasping platform without any grasping object. After placing the grasping object, the trained YOLOv4 model is used to identify the target to be grasped from the RGB image. The bounding box is taken as the region of interest, and the depth image portion corresponding to the pixels of the region of interest is extracted and replaced with the corresponding pixels of the blank image, thus obtaining a depth image that excludes other interfering objects and only contains the grasping target. This image is then processed and input into the GGCNN network, where a transformation from the image coordinate system to the robotic arm's body coordinate system is performed. Finally, the optimal robotic arm grasping scheme is output, and the robotic arm grasps the target objects sequentially according to the grasping scheme. However, although this technical solution uses YOLO detection to guide depth processing, its core relies on the subsequent complete GGCNN network to predict and score pixel-level grasping points on the cropped depth image. The network model still needs to process the entire target depth map, which has a relatively large computational load. In addition, the process includes extra steps such as image replacement and cropping. There is still room for improvement in overall computational efficiency and real-time performance, and its adaptability to edge computing platforms is insufficient.

[0004] In the prior art, Chinese patent document CN114902872A discloses a "visual guidance method for robot fruit picking," which acquires RGB and depth images using a Realsense D435i sensor; uses a target detection neural network to identify and locate fruits in the RGB images, and extracts the detection box region as the ROI image; segments the block-shaped distribution data on the fruit surface in the depth image detection box to achieve instance segmentation and obtain a mask image of the fruit; solves the normal vector of each point on the fruit surface in the mask image and depth image based on partial differential equations in spherical coordinates; solves the fruit picking position and rotation angle to provide the target fruit position and rotation angle for the robot to perform picking operations; and sends the position and rotation angle information to the robotic arm to control the robotic arm's motion planning to achieve the picking task. However, this technical solution is geared towards agricultural picking scenarios and is not designed for the stringent requirements of real-time performance and reliability in industrial environments. Its posture calculation is based on partial differential equations in spherical coordinates, resulting in a relatively complex calculation path, and it does not optimize efficiency for planar objects such as batteries. Furthermore, the solution completely lacks a safety verification and interception mechanism for abnormal target pose, and cannot prevent the robotic arm from failing to grasp or colliding in dangerous situations such as battery tilt.

[0005] In summary, existing technologies suffer from several problems: they cannot perceive the three-dimensional posture and physical volume of the battery, rely on physical markers, have low recognition rates in outdoor environments with strong light and wear, resulting in a high failure rate and poor safety for 4-DOF robotic arms in non-planar conditions, and the existing models have high computational complexity, large memory consumption, and significant system processing delays. Summary of the Invention

[0006] This invention solves the technical problems of existing technologies, such as the inability to perceive the three-dimensional posture and physical volume of the battery, reliance on physical markers, low recognition rate in outdoor strong light and wear environments, resulting in high failure rate and poor safety of 4-DOF robotic arms in non-planar working conditions, as well as high computational complexity, large memory consumption and significant system processing delay.

[0007] The pose estimation method based on ROS2 described in this invention includes the following steps: Step 1, acquiring and synchronizing the depth map and RGB image of the UAV through a camera; Step 2, preprocessing the RGB image, using an object detection model to identify the preprocessed RGB image, selecting the best bounding box for the category of battery, and constructing the ROI region based on the best bounding box; Step 3, generating a mask with the same size as the depth map, and using the mask to obtain the depth sub-matrix of the ROI region from the depth map; Step 4, performing double filtering on the depth sub-matrix to obtain a smoothed depth map, calculating the battery position based on the smoothed depth map, calculating the battery surface normal vector based on the battery position, and calculating its projection angle on the XZ plane based on the unit normal vector of the battery surface to obtain the battery rotation angle.

[0008] Furthermore, in one embodiment of the present invention, the RGB image is preprocessed in step 2 to perform CUDA-accelerated adaptive Gamma correction on the RGB image.

[0009] Furthermore, in one embodiment of the present invention, step 2 performs non-maximum suppression on the identification results to filter out the confidence levels. And the best bounding box is for the category of battery.

[0010] Furthermore, in one embodiment of the present invention, the construction of the ROI region based on the optimal bounding box in step 2 specifically involves: extracting the pixel coordinates of the optimal bounding box, expanding the optimal bounding box by several pixels, and constructing the ROI region.

[0011] Furthermore, in one embodiment of the present invention, the double filtering of the depth submatrix in step 4 specifically involves: thresholding the depth submatrix to remove background and invalid zero values, and then performing median filtering on the thresholded depth submatrix to obtain a smooth depth map.

[0012] Furthermore, in one embodiment of the present invention, the calculation of the battery surface normal vector based on the battery position in step 4 specifically involves: extracting the geometric center pixel of the ROI region, selecting any two neighboring pixels of the geometric center pixel whose vectors are linearly independent of the center point, calculating the spatial vector between the center point and one of the neighboring pixels, and the spatial vector between the center point and the other neighboring pixel, and calculating the battery surface normal vector based on the two spatial vectors.

[0013] The ROS2-based robotic arm guidance method of this invention is implemented based on the above method and includes the following steps: Step 1, converting the battery position and battery surface normal vector to the robotic arm base coordinate system; Step 2, calculating the required grasping yaw angle of the robotic arm end effector based on the battery surface normal vector converted in Step 1, and performing attitude safety verification; Step 3, controlling the robotic arm to grasp the target battery based on the battery position and grasping yaw angle corresponding to the attitude safety verification.

[0014] Furthermore, in one embodiment of the present invention, the attitude safety verification in step 2 specifically involves: calculating the converted battery surface normal vector. With the Z-axis of the world coordinate system The included angle If the included angle If the battery tilt angle is determined to be outside the flexible adsorption range of the 4-axis robotic arm, a TILT_ERROR exception is triggered, the host computer will prohibit the issuance of motion commands and issue an audible and visual alarm.

[0015] The pose estimation system based on ROS2 described in this invention is constructed based on the above method and includes the following modules: an acquisition module, which acquires and synchronizes the depth map and RGB map of the UAV through a camera; a construction module, which preprocesses the RGB map, uses an object detection model to identify the preprocessed RGB map, selects the best bounding box for the category of battery, and constructs the ROI region based on the best bounding box; a generation module, which generates a mask with the same size as the depth map, and uses the mask to obtain the depth sub-matrix of the ROI region from the depth map; and a solution module, which performs double filtering on the depth sub-matrix to obtain a smoothed depth map, calculates the battery position based on the smoothed depth map, calculates the battery surface normal vector based on the battery position, and calculates its projection angle on the XZ plane based on the unit normal vector of the battery surface to obtain the battery rotation angle.

[0016] The ROS2-based robotic arm guidance system of this invention is constructed based on the above method and includes the following modules: a conversion module, which converts the battery position and battery surface normal vector to the robotic arm base coordinate system; a verification module, which calculates the required grasping yaw angle of the robotic arm end effector based on the battery surface normal vector converted by the conversion module and performs attitude safety verification; and an execution module, which controls the robotic arm to grasp the target battery based on the battery position and grasping yaw angle corresponding to the attitude safety verification.

[0017] This invention solves the technical problems of existing technologies, such as the inability to perceive the three-dimensional posture and physical volume of the battery, reliance on physical markers, low recognition rate in outdoor strong light and wear environments, resulting in high failure rate and poor safety of 4-DOF robotic arms in non-planar working conditions, as well as high computational complexity, large memory consumption and significant system processing delay. Specific beneficial effects include: 1. This invention proposes a pose estimation method based on ROS2. The target detection model is only used for coarse localization of two-dimensional targets. A mask is generated by constructing the ROI (Region of Interest) based on the optimal bounding box. Calculations are performed directly on the two-dimensional matrix of the depth map, without the need to generate and process complete point cloud data. This significantly reduces computational load and memory consumption, making it very suitable for edge computing platforms. It improves the overall real-time performance of the system and achieves rapid and accurate perception of the battery's three-dimensional pose and physical volume, overcoming the limitation of traditional algorithms that cannot perceive three-dimensional pose while reducing computational load. 2. This invention proposes a pose estimation method based on ROS2. Through pure vision-based deep learning target detection and pose calculation methods, it directly perceives the battery's three-dimensional pose and physical volume without the need for physical markers. This fundamentally solves the problem of physical marker failure in complex outdoor conditions, significantly improving the environmental adaptability and reliability of the 4-DOF robotic arm in non-planar conditions. 3. This invention proposes a robotic arm guidance method based on ROS2, introducing a safety verification mechanism based on pose calculation results. This mechanism can automatically identify and intercept grasping risks caused by excessive platform tilt, improving the safety and success rate of the entire operation process. Attached Figure Description

[0018] The above and / or additional aspects and advantages of the present invention will become apparent and readily understood from the following description of embodiments taken in conjunction with the accompanying drawings, in which: Figure 1 is a flowchart of system initialization and communication establishment according to Embodiment 1; Figure 2 is a flowchart of preprocessing and mask generation, depth map dual filtering and position and pose calculation according to Embodiment 1; Figure 3 is a flowchart of robot arm angle and normal tilt angle calculation according to Embodiment 5; Figure 4 is a flowchart of adaptive visual servoing and flexible execution control according to Embodiment 5; Figure 5 is a schematic diagram of ROI region preprocessing effect and depth submatrix extraction according to Embodiment 2. Detailed Implementation

[0019] Various embodiments of the present invention will now be clearly and completely described with reference to the accompanying drawings. The embodiments described with reference to the drawings are exemplary and intended to explain the present invention, and should not be construed as limiting the present invention.

[0020] Implementation Method 1: Due to the limited space of the fully automated drone battery swapping platform, the limited number of batteries in the hangar and drone docking area are planar rectangular structures. The robotic arm approaches from the tail of the drone and attaches to the short side of the battery. The grasping task only requires the vacuum suction cup at the end to adhere to the upper surface of the battery. Therefore, only four degrees of freedom are required: X, Y, and Z three-dimensional translation and yaw rotation around the Z-axis. Compared to a 6-DOF robotic arm, the 4-DOF solution can reduce hardware costs by 30%-50%, significantly reduce the amount of inverse kinematics calculations and path planning complexity, and reduce system power consumption and collision risk. It is also more suitable for edge computing platforms and outdoor high-frequency battery swapping scenarios. If a 6-DOF arm is used, although it can handle any posture, it will be difficult to achieve large-scale deployment due to high cost, complex control, and increased energy consumption. However, existing drone automatic battery swapping systems rely on physical markers such as AprilTag (visual reference marking system), which have low recognition rates in outdoor strong light and abrasion environments. Furthermore, the communication latency based on the ROS1 architecture is uncontrollable, and traditional visual algorithms cannot perceive the three-dimensional posture and physical volume of the battery. This results in a high failure rate and poor safety of the 4-DOF robotic arm in non-planar working conditions.

[0021] To address the aforementioned technical issues, this implementation proposes a pose estimation method based on ROS2, running in a control system equipped with an edge computing module. The first stage involves system construction and hardware calibration, focusing on building a ROS2-based software runtime environment and loading parameters. After system power-on, global parameters (camera intrinsics, hand-eye calibration matrix, etc.) are loaded, a high-speed communication domain based on DDS (Data Distribution Service) middleware is established, and camera and robotic arm drive nodes are started and QoS (Quality of Service) policies are configured. After broadcasting the static TF (Transformation) coordinate tree, the system enters standby mode, as shown in Figure 1, specifically including the following steps: Step 1, after system power-on, the ROS2 daemon process is started, and the lifecycle_manager node is loaded, responsible for managing the lifecycle of each functional node: Configuring, Inactive, Active, and Finalized.

[0022] Step 2: Read the YAML configuration file (a configuration file based on the YAML format) and store the camera intrinsic parameter matrix. Distortion coefficient ,in For radial distortion parameters, Tangential distortion parameters, hand-eye calibration matrix And the YOLO V11 model path is loaded into the global parameter server.

[0023] Step 3: Establish the DDS communication domain, configure ROS_DOMAIN_ID, and initialize Cyclone DDS (an open-source, lightweight data distribution service implementation) based on the XML configuration file (a file written in Extensible Markup Language), and enable SharedMemory transmission mode to optimize large data transmission between nodes.

[0024] This implementation method effectively reduces the latency of image transmission and inter-node communication by adopting the ROS2 distributed architecture and optimized DDS communication strategy, and solves the problem of uncontrollable latency in high-resolution image transmission in traditional ROS1-based systems, thereby meeting the high real-time requirements of battery swapping operations.

[0025] Step 4: Initialize the camera driver node, start orbbec_camera_node, enable hardware alignment between the depth map and the RGB image (color image), and publish the topics / camera / color / image_raw (RGB) and / camera / depth / image_raw (Depth).

[0026] Step 5: Initialize the model inference engine. In vision_inference_node, call the TensorRT API (deep learning inference acceleration library) to deserialize the FP16 quantized YOLO V11 engine model to GPU memory and pre-allocate inference memory space.

[0027] Step 6: Initialize the robotic arm communication node, start the arm_driver_node, establish a serial port connection with the robotic arm controller, reset the robotic arm error state, and start the status polling thread.

[0028] Step 7, configure the image QoS policy, subscribe to the image topic, and set the QoS to Reliability: BEST_EFFORT and History: KEEP_LAST(1) to ensure that the visual algorithm always processes the latest frame and discards old frames when the network is congested.

[0029] Step 8: For the robotic arm control command topic / arm / cmd_vel, set QoS Reliability:RELIABLE to ensure that each motion command is delivered accurately.

[0030] Based on the aforementioned software operating environment and parameters, a pose estimation method based on ROS2 is implemented. This method utilizes a deep learning network to quickly identify the target's two-dimensional features and employs a region locking mechanism. This lays the foundation for subsequent direct double filtering and neighborhood vector cross product normal calculation on the depth map's two-dimensional matrix, thereby reducing computational power consumption. As shown in Figure 2, the method includes the following steps: Step 1, upon receiving the battery swapping command, the RGB frame and depth frame are synchronized using the ApproximateTime strategy to ensure the timestamp difference between the two is minimized. ms; Step 2, preprocess the RGB image, input the preprocessed image into the YOLOv11 network accelerated by TensorRT, perform forward inference, output a feature tensor containing category, confidence and bounding box, filter out the best bounding box for category battery, and construct ROI region based on the best bounding box; Step 3, generate interest mask, create a binary mask of the same size as the depth map. Pixels within the ROI region are set to 1, and those in other regions are set to 0. The depth submatrix of the target region is then directly extracted from the original depth map using a mask. Step 4: Perform double filtering on the depth submatrix to obtain a smooth depth map. Calculate the battery position based on the smooth depth map. Calculate the battery surface normal vector based on the battery position. Calculate the projection angle of the battery onto the XZ plane based on the unit normal vector of the battery surface to obtain the battery rotation angle.

[0031] This implementation method organically integrates efficient deep learning recognition, lightweight 3D pose calculation, multi-coordinate system safety verification, and adaptive visual servo control. It not only solves the accuracy problem of a single link, but also improves the overall efficiency and reliability of fully automated battery swapping operations for UAVs through system-level optimization.

[0032] Implementation Method Two: The difference between this implementation method and Implementation Method One is that in step 2, the RGB image is preprocessed to perform CUDA-accelerated adaptive Gamma correction on the RGB image in the GPU. The formula is as follows: ;in, For input pixels, To output pixels, The correction coefficient is used to enhance the texture details of the black surface of the battery. As shown in Figure 5, it is a schematic diagram of the ROI region preprocessing effect and depth submatrix extraction.

[0033] Implementation Method 3: The difference between this implementation method and Implementation Method 1 is that non-maximum suppression (NMS) is applied to the identification results to filter out the confidence levels. And the best bounding box for the category "battery".

[0034] Extract the pixel coordinates of the optimal bounding box Expand outward by several pixels based on the optimal bounding box. (Optional increase in pixel count) Constructing a locked region of interest (ROI) is done using the following formula: .

[0035] Implementation Method 4: The difference between this implementation method and Implementation Method 1 is that step 4 involves double filtering of the depth submatrix, specifically including the following steps: Step 41, performing depth range truncation and traversing the depth submatrix. Apply threshold truncation: Directly remove background and invalid zero values, retaining valid depth pixels.

[0036] in, For the coordinates The original depth value at that location, These are pixel coordinates.

[0037] Step 42, for implement Median filtering of the window removes salt-and-pepper noise (isolated depth jumps) caused by light reflection, resulting in a smooth depth map. .

[0038] Step 4, which calculates the battery surface normal vector based on the battery location, specifically includes the following steps: Step 43, extracting the geometric center pixel of the ROI region. and its depth value The camera-frame spatial coordinates of the target are calculated using the pinhole model. : ;in, , The principal point coordinates of the camera. , These are the focal lengths of the camera in the x and y directions, respectively. These are the three-dimensional spatial coordinates in the camera coordinate system.

[0039] Step 44, in Randomly select two neighboring pixels that are linearly independent of the vector formed by the center point from the top, bottom, left, and right. and , respectively denoted as and Among them, step size Pixel.

[0040] Step 45: Using the formula from step 43, calculate the two spatial vectors formed by the center point and the neighboring points respectively: These two vectors approximately represent the tangent vectors of the battery surface in the X and Y directions.

[0041] Step 46: Using the cross product principle, directly calculate the unit normal vector of the battery surface. : ; Step 47: Based on the characteristics of the 4-DOF robotic arm, calculate the projection angle of the unit normal vector on the battery surface onto the XZ plane, and obtain the rotation angle in the camera frame. .

[0042] Step 48, calculate the position and rotation angle Package it as a geometry_msgs / PoseStamped message, and attach the current timestamp.

[0043] This implementation does not generate point clouds, but performs double filtering directly on the two-dimensional matrix of the depth map and directly solves the spatial pose using the pixel neighborhood difference method. This significantly reduces the amount of computation and memory usage, making it very suitable for edge computing platforms. It improves the overall real-time performance of the system and enables rapid and accurate perception of the battery's three-dimensional pose and physical volume, overcoming the limitation of traditional algorithms that cannot perceive three-dimensional pose.

[0044] Implementation Method 5: A robotic arm guidance method based on ROS2. This method is implemented based on the target recognition method described in Implementation Method 1, as shown in Figure 3, and includes the following steps: Step 1, transforming the battery position and battery surface normal vector to the robotic arm base coordinate system; retrieving the hand-eye calibration matrix from the TF tree. Position the battery Transform to the robot arm base coordinate system: Using the rotation matrix in the hand-eye calibration matrix Transform the normal vector to the base coordinate system: .

[0045] Step 2, based on the converted battery surface normal vector Calculate the gripping yaw angle required at the end effector of the robotic arm. : Constructing state vectors The solution results for consecutive frames are smoothed using a linear Kalman filter, and the covariance matrix is... Set as Suppress measurement noise jitter. This represents the center position of the battery in the coordinate system of the robotic arm base.

[0046] Step 3: Based on the battery position and grasping yaw angle corresponding to the attitude safety verification, control the robotic arm to grasp the target battery, as shown in Figure 4. This includes the following steps: Step 31: After the verification is passed, the host computer plans the movement from the current pose to the hovering point. The path. To prevent battery pose deviation caused by sudden stops and starts of the robotic arm, a five-segment S-shaped velocity curve is used to plan the joint velocity. : ;in, To accelerate and ensure smooth, vibration-free movement, the robotic arm is driven to the hovering point.

[0047] Step 32: Upon reaching the hovering point, the state machine switches to SERVOING (servo control mode). At this point, the YOLOv11 ROI region is locked, and the grab yaw angle calculated in the previous steps is used. The robot arm's end effector rotates so that the long side of its suction cup is quickly aligned with the direction of the battery handle via a PD attitude (proportional-derivative control attitude) controller.

[0048] Step 33: Activate 30Hz high-frequency closed-loop control. Decouple horizontal and vertical motion. Horizontal direction. Employing a servo control law based on image moments: ;in, The horizontal speed command is for the camera system. It is an adaptive gain (which decreases as depth decreases). The pseudo-inverse of the image Jacobian matrix, This represents the pixel error between the current feature point and the image center point.

[0049] Step 34: Instead of using simple image errors in the Z-axis direction, the real-time distance feedback from the depth camera is utilized. Construct an exponentially decaying rate of decline model: Ensure that the robotic arm descends rapidly when it is far from the target, and automatically decelerates to a creeping speed when it approaches the target (<5cm) to achieve a soft landing.

[0050] Step 35, Real-time monitoring of depth and distance .when When the physical contact threshold is reached, a Stop command is immediately sent to pause the Z-axis movement, and the I / O module is simultaneously triggered to close the vacuum pump relay.

[0051] Step 36: To prevent misjudgment due to air pressure fluctuations, a hysteresis comparator logic is used to detect negative pressure. An adsorption threshold is set. Release threshold Only when the sensor feedback value And the duration If the condition is met, the status is determined to be LOCKED (adsorption successful); otherwise, it remains RETRY (retry) or an error is reported.

[0052] Step 37: After successful adsorption, a "spring-damping" system is simulated to perform a lifting action. Within the initial 50mm stroke of the Z-axis lift, the maximum acceleration is limited to prevent the battery from slipping off the suction cup due to overload; then, a "reverse-rotation" composite trajectory is executed at maximum speed to return to the storage.

[0053] Step 38: During the servoing process, if visual features are continuously lost for more than 5 frames, or the control speed calculated by PBVS (Position-Based Visual Servoing) fails... If the safety threshold is exceeded, the watchdog logic immediately takes over control, forcibly executes a reset action of raising the Z-axis by 100mm, and reports the SERVO_LOST error code.

[0054] Implementation Method Six: The difference between this implementation method and Implementation Method Five is that the attitude safety verification in step 2 specifically involves: calculating the converted battery surface normal vector. With the Z-axis of the world coordinate system The included angle : If the included angle If the battery tilt angle is determined to be outside the flexible adsorption range of the 4-axis robotic arm, a TILT_ERROR exception is triggered, the host computer will prohibit the issuance of motion commands and issue an audible and visual alarm.

[0055] Implementation Method Seven: This implementation method is a specific embodiment of the implementation methods one through six.

[0056] Example 1: 1. The hardware environment of this example includes: The hardware execution part uses a Pfide 57BYG250D-8 four-degree-of-freedom collaborative robotic arm, with a customized vacuum adsorption fixture mounted on its end flange. A high-sensitivity digital pressure sensor is integrated inside the fixture to provide feedback on the adsorption status. Vision sensor: A Gemini Max depth camera is rigidly fixed to an aluminum profile bracket above the battery swapping platform using an "eye outside the hand" mounting method. The camera's optical axis is tilted downwards at approximately 45 degrees, providing complete field of view covering the drone docking platform and the robotic arm's working area. Computing unit: An NVIDIA Jetson Orin Nano edge computing module is used, running the Ubuntu 22.04 operating system and ROS2 Humble middleware.

[0057] 2. Software Communication Architecture This system is developed based on the ROS2 distributed architecture: CycloneDDS is selected as the underlying communication middleware for DDS configuration. For the image topic / camera / aligned / image_raw, the QoS policy is configured as Reliability: BEST_EFFORT to ensure high-bandwidth transmission; for the pose control command / arm / target_pose, the QoS policy is configured as Reliability: RELIABLE to ensure command delivery. Regarding the coordinate system, the system pre-calibrates the hand-eye coordinates using a ChArUco calibration board to obtain the static transformation matrix of the camera optical center coordinate system relative to the robotic arm base coordinate system. And it is published through the tf2_ros static broadcaster.

[0058] Example 2: Replacement of the visual sensor: Although Example 1 used a stereo camera, Gemini Max, this example is also applicable to ToF (Time of Flight) cameras, such as Azure Kinect or the structured light-based RealSense camera. As long as the sensor can provide pixel-aligned RGB-D data streams, the point cloud-free pose calculation method described in this example can be applied.

[0059] Replacement of robotic arm type: Example 1 uses a four-degree-of-freedom collaborative robotic arm. In space-constrained scenarios, it can also be replaced with a 4-axis SCARA robotic arm, provided that the battery surface is basically level and only Z-axis rotation alignment is required, or a traditional industrial robot, such as ABB or Kuka, which requires the corresponding ROS2 drive package.

[0060] Replacement of computing platform: The edge computing unit can be replaced with an industrial control computer based on the x86 architecture. Only the TensorRT engine files of YOLO V11 need to be recompiled. The node code of ROS2 has cross-platform compatibility.

[0061] Implementation Method 8: A pose estimation system based on ROS2. This system is constructed based on the method described in Implementation Method 1 and includes the following modules: an acquisition module, which acquires and synchronizes the depth map and RGB map of the UAV via a camera; a construction module, which preprocesses the RGB map, uses a target detection model to identify the preprocessed RGB map, selects the optimal bounding box for the battery category, and constructs the ROI region based on the optimal bounding box; a generation module, which generates a mask of the same size as the depth map and uses the mask to obtain the depth sub-matrix of the ROI region from the depth map; and a solution module, which performs double filtering on the depth sub-matrix to obtain a smoothed depth map, calculates the battery position based on the smoothed depth map, calculates the battery surface normal vector based on the battery position, and calculates its projection angle on the XZ plane based on the unit normal vector of the battery surface to obtain the battery rotation angle.

[0062] Implementation Method Nine: A ROS2-based robotic arm guidance system, which is constructed based on the method described in Implementation Method Five, and includes the following modules: a conversion module, which converts the battery position and battery surface normal vector to the robotic arm base coordinate system; a verification module, which calculates the required grasping yaw angle at the end of the robotic arm based on the battery surface normal vector converted by the conversion module, and performs attitude safety verification; and an execution module, which controls the robotic arm to grasp the target battery based on the battery position and grasping yaw angle corresponding to the attitude safety verification.

[0063] The above provides a detailed description of the pose estimation method and system based on ROS2, as well as the guidance method and system proposed in this invention. Specific examples have been used to illustrate the principles and implementation methods of this invention. The descriptions of the above embodiments are only for the purpose of helping to understand the method and core ideas of this invention. At the same time, for those skilled in the art, there will be changes in the specific implementation methods and application scope based on the ideas of this invention. Therefore, the content of this specification should not be construed as a limitation of this invention.

Claims

1. A pose estimation method based on ROS2, characterized in that, Includes the following steps: Step 1: Acquire and synchronize the depth map and RGB image of the UAV using the camera; Step 2: Preprocess the RGB image, use an object detection model to identify the preprocessed RGB image, select the best bounding box for the category of battery, and construct the ROI region based on the best bounding box; Step 3: Generate a mask with the same size as the depth map, and use the mask to obtain the depth sub-matrix of the ROI region from the depth map; Step 4: Perform double filtering on the depth sub-matrix to obtain a smooth depth map, calculate the battery position based on the smooth depth map, calculate the battery surface normal vector based on the battery position, and calculate its projection angle on the XZ plane based on the unit normal vector of the battery surface to obtain the battery rotation angle.

2. The pose estimation method based on ROS2 according to claim 1, characterized in that, In step 2, the RGB image is preprocessed to perform CUDA-accelerated adaptive Gamma correction on the RGB image.

3. The pose estimation method based on ROS2 according to claim 1, characterized in that, Step 2 performs non-maximum suppression on the identification results to filter out the confidence levels. And the best bounding box is for the category of battery.

4. The pose estimation method based on ROS2 according to claim 1, characterized in that, In step 2, the ROI region is constructed based on the optimal bounding box. Specifically, the pixel coordinates of the optimal bounding box are extracted, and the ROI region is constructed by expanding the optimal bounding box by several pixels.

5. The pose estimation method based on ROS2 according to claim 1, characterized in that, In step 4, the depth submatrix is ​​subjected to dual filtering, specifically: the depth submatrix is ​​truncated with a threshold to remove background and invalid zero values, and the truncated depth submatrix is ​​then subjected to median filtering to obtain a smooth depth map.

6. The pose estimation method based on ROS2 according to claim 1, characterized in that, In step 4, the battery surface normal vector is calculated based on the battery location. Specifically, the geometric center pixel of the ROI region is extracted, and any two neighboring pixels of the geometric center pixel that are linearly independent of the vector formed by the center point are selected. The spatial vector between the center point and one of the neighboring pixels, and the spatial vector between the center point and the other neighboring pixel are calculated respectively. The battery surface normal vector is calculated based on the two spatial vectors.

7. A robotic arm guidance method based on ROS2, wherein the method is implemented based on the method described in claim 1, characterized in that, Includes the following steps: Step 1: Transform the battery position and battery surface normal vector into the coordinate system of the robotic arm base; Step 2: Calculate the required grasping yaw angle at the end of the robotic arm based on the battery surface normal vector transformed in Step 1, and perform attitude safety verification; Step 3: Based on the battery position and grasping yaw angle corresponding to the attitude safety verification, control the robotic arm to grasp the target battery.

8. The ROS2-based robotic arm guidance method according to claim 7, characterized in that, The attitude safety verification in step 2 specifically involves: calculating the converted battery surface normal vector. With the Z-axis of the world coordinate system The included angle If the included angle If the battery tilt angle is determined to be outside the flexible adsorption range of the 4-axis robotic arm, a TILT_ERROR exception is triggered, the host computer will prohibit the issuance of motion commands and issue an audible and visual alarm.

9. A pose estimation system based on ROS2, characterized in that, The system is constructed based on the method described in claim 1. It includes the following modules: an acquisition module, which acquires and synchronizes the depth map and RGB map of the drone through the camera; and a construction module, which preprocesses the RGB map, uses an object detection model to identify the preprocessed RGB map, selects the best bounding box for the category of battery, and constructs the ROI region based on the best bounding box. The generation module generates a mask with the same size as the depth map, and uses the mask to obtain the depth submatrix of the ROI region from the depth map. The solution module performs double filtering on the depth submatrix to obtain a smooth depth map. Based on the smooth depth map, it calculates the battery position, calculates the battery surface normal vector based on the battery position, and calculates the projection angle of the battery onto the XZ plane based on the unit normal vector of the battery surface to obtain the battery rotation angle.

10. A ROS2-based robotic arm guidance system, said system being constructed based on the method of claim 7, characterized in that, Includes the following modules: a transformation module that transforms the battery position and battery surface normal vector to the robot arm base coordinate system; The verification module calculates the required grasping yaw angle at the end of the robotic arm based on the battery surface normal vector converted by the conversion module, and performs attitude safety verification; the execution module controls the robotic arm to grasp the target battery based on the battery position and grasping yaw angle corresponding to the attitude safety verification.

Images