High-fidelity visual positioning system and method for condition observation of high-vibration platforms

Abstract

This invention proposes a high-fidelity state observation visual positioning system and method for high-vibration platforms, belonging to the field of machine vision and image processing, and applicable to the external closed-loop control of high-vibration platforms such as robotic arms. Addressing the problems of measurement failure due to motion fuzziness, high data noise, and multi-sensor calibration errors in existing technologies, this invention employs a single RGBD camera (with a field of view covering both the external target and the robotic arm's visual labels), combined with a process of "deep learning to lock the region of interest – high-precision algorithm to measure pose – multi-label weighted fusion – temporal low-pass smoothing" to output a high-fidelity pose signal. This invention eliminates multi-sensor calibration errors, has strong anti-fuzziness capabilities, and provides smooth signal adaptation to control requirements, improving the robotic arm positioning accuracy to the 3mm level and significantly increasing the success rate of dynamic tasks.

Description

Technical Field

[0001] This invention belongs to the field of machine vision and image processing technology, specifically relating to visual perception and pose measurement technology for high dynamic and high vibration platforms (such as drones equipped with lightweight robotic arms). More precisely, it is a visual positioning system and method that provides high-precision, high-stability, and high-fidelity pose state observation data for the external closed-loop control system (such as dynamic grasping control) of such platforms. Background Technology

[0002] In robotics, especially in automated systems like drones, data-driven external closed-loop control strategies are crucial for achieving high-precision dynamic tasks. The performance of such control strategies depends entirely on the quality of the data provided by their feedback source—typically a vision system. An ideal visual feedback signal must be accurate, continuous, and reliable.

[0003] However, deploying visual positioning systems on high-vibration platforms such as drones presents significant challenges for existing technologies. First, the platform's high-frequency vibrations and rapid maneuvers generate severe motion blur, drastically reducing image quality and causing traditional high-precision measurement algorithms (such as April Tag) that rely on sharp edges and feature points to experience decreased recognition rates or even complete failure. Second, even if the visual algorithm can calculate the pose, its raw output data stream typically contains a large amount of high-frequency noise and outliers caused by image noise, algorithm jitter, etc. Using this unprocessed, coarse data directly as feedback signals for the control system will inevitably lead to violent oscillations in control commands, affecting the stability of the mechanical structure and potentially causing the entire system to diverge. Finally, if separate cameras are used to observe external targets and the robot's own components (such as a robotic arm), complex hand-eye calibration is necessary. This calibration process is not only cumbersome, but its inherent calibration errors become an insurmountable bottleneck to the overall system accuracy, limiting the final positioning precision.

[0004] Therefore, how to obtain high-quality visual feedback signals that can be used for precision closed-loop control under harsh working conditions of strong vibration and high dynamics is a technical problem that urgently needs to be solved in this field. Summary of the Invention

[0005] This invention provides a high-fidelity visual positioning system and method for state observation on high-vibration platforms. It addresses the problem that existing technologies, under high-vibration and high-dynamic environments, struggle to provide accurate, stable, and continuous pose feedback signals that can be directly used for closed-loop control due to issues such as motion ambiguity, signal noise, and multi-sensor calibration errors.

[0006] The core technology of this invention mainly involves a hardware design that uses a single camera to simultaneously observe external targets and its own end effector, combined with a cascaded software process of "lock-measure-fusion-smoothing" to stably extract and output high-fidelity pose information from a video stream that is blurred due to vibration.

[0007] In a first aspect, the present invention provides a high-fidelity state observation visual positioning system for a high-vibration platform, used to provide pose observation data for the external closed-loop control system of the high-vibration platform, including:

[0008] A single integrated vision module, a camera with color image acquisition and depth data acquisition functions, is installed in a position and field of view that can simultaneously cover external targets and at least one vision tag installed on the end effector of the robotic arm mounted on a high-vibration platform.

[0009] The anti-blurring module includes a locking unit and a measurement unit. The locking unit is configured to use a deep learning target detection algorithm with strong generalization ability to process the original video frames acquired by the single integrated vision module, and robustly lock the regions of interest corresponding to the external target and the visual label. The measurement unit is configured to use a high-precision pose estimation algorithm to process the regions of interest output by the locking unit, and calculate the three-dimensional coordinates of the external target and the six-degree-of-freedom pose of the visual label.

[0010] The multi-label pose fusion module is configured to acquire the pose data of all visible visual labels calculated by the measurement unit, and to perform weighted fusion of the translation vector and attitude parameters of the pose data according to the preset confidence weight, and output the fused pose data.

[0011] The temporal smoothing module is configured to use a temporal low-pass filtering algorithm to process the fused pose data output by the multi-tag pose fusion module, filter out high-frequency noise and data jitter, and obtain a smooth pose signal.

[0012] The data interaction module is configured to transmit the smoothed pose signal output by the time-domain smoothing module to the external closed-loop control system, providing state observation feedback to the external closed-loop control system.

[0013] Furthermore, a single integrated vision module is mounted under the fuselage of a high-vibration platform, which is a drone equipped with a robotic arm.

[0014] Furthermore, the deep learning object detection algorithm used by the locking unit is the YOLO series object detection algorithm. The YOLO series object detection algorithm loads the pre-trained model through a dedicated inference engine and performs image inference, outputting the coordinate range of the region of interest.

[0015] Furthermore, the high-precision pose estimation algorithm used by the measurement unit is the April Tag decoding and pose calculation algorithm. The April Tag decoding and pose calculation algorithm, combined with the camera-involved distortion coefficients of the single integrated vision module, calculates the six-degree-of-freedom pose of the visual tag in the coordinate system of the single integrated vision module.

[0016] Furthermore, the confidence weights preset in the multi-label pose fusion module are determined based on the physical size of the visual labels. The larger the physical size of the visual label, the higher the confidence weight. The weighted fusion of pose parameters includes first aligning the pose quaternions of all visual labels to the same reference hemisphere, and then performing weighted summation and normalization.

[0017] Furthermore, the temporal smoothing module uses an exponential moving average algorithm to filter the translation vector in the fused pose data, and a spherical linear interpolation algorithm to filter the attitude parameters in the fused pose data.

[0018] Secondly, the present invention provides a high-fidelity visual positioning method for condition observation of a high-vibration platform, comprising the following steps:

[0019] S1. Deploy a single integrated vision module: Install a camera with color image acquisition and depth data acquisition functions on a high-vibration platform so that the camera's field of view simultaneously covers the external target and at least one visual tag on the end effector of the robotic arm.

[0020] S2. Anti-blurring processing: Locking and measurement operations are performed on the original video frames acquired by the single integrated vision module; the locking operation uses a deep learning target detection algorithm with strong generalization ability to robustly lock the region of interest of the external target and the visual label; the measurement operation uses a high-precision pose estimation algorithm to calculate the three-dimensional coordinates of the external target and the six-degree-of-freedom pose of the visual label based on the region of interest.

[0021] S3. Multi-label pose fusion: Obtain the pose data of all visible visual labels solved in step S2, and perform weighted fusion of the translation vector and pose parameters of the pose data according to the preset confidence weight to obtain fused pose data;

[0022] S4. Temporal smoothing: A temporal low-pass filtering algorithm is used to process the fused pose data to remove high-frequency noise and data jitter, generating a smooth pose signal;

[0023] S5. Data Output: Transmits smooth pose signals to the external closed-loop control system of the high-vibration platform, providing high-fidelity state observation data for the external closed-loop control system.

[0024] Furthermore, in step S2, the deep learning object detection algorithm is the YOLO series algorithm. The YOLO series algorithm loads a pre-trained model and performs inference on dedicated inference hardware to output the pixel coordinate range of the region of interest.

[0025] Furthermore, in step S2, the high-precision pose estimation algorithm is the April Tag algorithm. The April Tag algorithm combines the intrinsic parameter data of a single integrated vision module to perform corner detection and decoding on the visual labels in the region of interest, and calculates the six-degree-of-freedom pose of the visual labels.

[0026] Furthermore, the confidence weight preset in step S3 is positively correlated with the physical size of the visual label. The larger the physical size of the visual label, the higher the confidence weight. The weighted fusion of pose parameters includes first determining the sign consistency between the pose quaternion of each visual label and the reference quaternion, aligning them to the same hemisphere, and then performing weighted calculation.

[0027] Furthermore, the time-domain low-pass filtering algorithm in step S4 includes: applying exponential moving average filtering to the translation vector of the fused pose data, and applying spherical linear interpolation filtering to the attitude parameters of the fused pose data, wherein the smoothing factor of the exponential moving average filtering is set according to the vibration frequency of the high vibration platform and the response requirements of the external closed-loop control system.

[0028] The main contributions and innovations of this invention are as follows:

[0029] 1. Front-end and back-end collaborative design effectively overcomes measurement challenges: This invention proactively combats the impact of vibration ambiguity on information acquisition through a front-end "lock-measure" process, while simultaneously performing deep signal purification and noise reduction through a back-end "fusion-smoothing" process. This front-end and back-end collaborative design fundamentally solves the data quality problem under harsh operating conditions.

[0030] 2. Significantly improved robustness and reliability: By performing pose-weighted fusion of multiple visual tags on the robotic arm, the system can effectively suppress errors and jumps caused by poor viewing angle, partial occlusion, or measurement failure of a single tag, ensuring the continuity and high reliability of the feedback signal.

[0031] 3. Eliminate calibration errors and simplify system integration: By adopting a single integrated vision module that takes into account both internal and external factors, the system completes the measurement of external targets and its own state under a unified sensor coordinate system. This physically eliminates the hand-eye calibration error between multiple sensors, which not only improves the system accuracy but also simplifies hardware configuration and deployment.

[0032] 4. Enabling Advanced Control and Enhancing Task Performance: The smooth, reliable, and high-frequency "control-friendly" pose data stream output by this invention is a necessary prerequisite for the stable and accurate operation of upper-level advanced control algorithms. Actual tests show that this invention can improve dynamic positioning accuracy from the centimeter level of the hardware itself to the millimeter level (3cm to 3mm), and significantly improve the success rate of high-difficulty tasks such as dynamic grasping.

[0033] Details of one or more embodiments of the present invention are set forth in the following drawings and description, so that other features, objects and advantages of the invention will be more readily understood. Attached Figure Description

[0034] The accompanying drawings, which are included to provide a further understanding of the invention and form part of this invention, illustrate exemplary embodiments of the invention and are used to explain the invention, but do not constitute an undue limitation of the invention. In the drawings:

[0035] Figure 1 This is an architecture diagram of a high-fidelity condition observation visual positioning system for a high-vibration platform according to an embodiment of the present invention.

[0036] Figure 2 This is a flowchart of a high-fidelity condition observation visual positioning method for a high-vibration platform according to an embodiment of the present invention;

[0037] Figure 3 This is an application diagram of a high-fidelity state observation visual positioning system according to an embodiment of the present invention;

[0038] Figure 4 This is a schematic diagram of an outdoor test according to an embodiment of the present invention;

[0039] Figure 5 This is a real-time image based on an embodiment of the present invention (with added translation and annotation);

[0040] Figure 6 This is a schematic diagram illustrating a successful outdoor capture test according to an embodiment of the present invention.

[0041] In the diagram, 1. Mobile platform; 2. Operating device; 3. Single integrated vision module; 4. Visual label; 5. Golf ball. Detailed Implementation

[0042] Exemplary embodiments will now be described in detail, examples of which are illustrated in the accompanying drawings. When the following description relates to the drawings, unless otherwise indicated, the same numerals in different drawings denote the same or similar elements. The embodiments described in the following exemplary embodiments do not represent all embodiments consistent with one or more embodiments of this specification. Rather, they are merely examples of apparatuses and methods consistent with some aspects of one or more embodiments of this specification as detailed in the appended claims.

[0043] It should be noted that the steps of the corresponding methods are not necessarily performed in the order shown and described in this specification in other embodiments. In some other embodiments, the methods may include more or fewer steps than described in this specification. Furthermore, a single step described in this specification may be broken down into multiple steps in other embodiments; and multiple steps described in this specification may be combined into a single step in other embodiments.

[0044] Example 1

[0045] The visual positioning system constructed in this embodiment is fundamentally positioned as a "high-fidelity state observer" for the UAV's external closed-loop control system (such as dynamic grasping control). Through an architecture of "single integrated vision module + cascaded software processing," the system stably extracts the "manipulator end effector pose" and the "position of external target objects (such as golf balls)" from the blurred video stream caused by the UAV's high-frequency vibrations, outputting smooth and accurate pose signals to support the manipulator in completing dynamic grasping tasks during flight. Specifically, this system includes a single integrated vision module 3, an anti-blurring processing module, a multi-label pose fusion module, a temporal smoothing module, and a data interaction module.

[0046] The external closed-loop control system mentioned in this invention transforms the spatial domain strong coupling problem between the mobile platform 1 (such as a drone) and the operating device 2 (such as a robotic arm) into a time-series scheduling problem through "time-domain decoupling." It is compatible with the closed underlying controller using a "dual black-box" architecture, without relying on dynamic models or modifying the underlying firmware. It is a system that achieves highly robust adaptive cooperative control by dynamically adjusting the task time process. This can be existing technology (i.e., the purpose of this invention is to provide data to the existing external closed-loop control system, rather than the external closed-loop control system itself), and is not the technical point to be protected by this invention. Therefore, it will not be described in detail.

[0047] In this embodiment, as Figure 3 As shown in Table 1, the hardware deployment, specific selection, and installation methods in this embodiment ensure both comprehensive field of view coverage and calibration error-free characteristics.

[0048] Table 1

[0049]

[0050] The single integrated vision module 3 uses an Orbbec Gemini 335 3D RGBD camera as its single integrated vision module. This camera simultaneously possesses color image acquisition (e.g., resolution 1280×720, frame rate 30fps) and depth data acquisition (e.g., measurement range 0.1-20m, depth accuracy ±1.5%) functions, meeting the requirements of "color image acquisition and depth data acquisition functions". The camera is installed under the drone body: a custom bracket is used to fix the camera directly under the drone body, with the lens facing downwards. The bracket angle is adjusted so that the camera's field of view simultaneously covers two areas: ① external targets below the drone (e.g., a standard golf ball 5 with a diameter of 42mm); ② the end effector of the robotic arm mounted on the drone (covering a 10cm×10cm area at the end, ensuring that the visual tag 4 is fully visible). This installation method physically achieves "measurement of external targets and robotic arm end effectors in the same sensor coordinate system", eliminating multi-sensor hand-eye calibration errors.

[0051] robotic arms: such as Figure 5 As shown, the Wafer Snow Electronics RoArm-M2-Pro lightweight robotic arm is selected, which has 4 degrees of freedom (shoulder rotation, shoulder swing, elbow swing, and wrist rotation). With a load of 100g, the open-loop positioning accuracy is approximately 3cm. Multiple (e.g., gripper grippers) are attached to the surface of the end effector. Figure 5 The five April Tag visual tags correspond to the feature of "at least one visual tag" in the text;

[0052] Drone platform: The DJI Mavic 3 Enterprise drone can be used, which has a hovering accuracy of ±0.1m (GPS mode) and is equipped with NVIDIA Jetson Orin Nano Super (for running deep learning inference) to provide computing power support for the system.

[0053] Regarding the software component, the software implementation of this invention is a standalone, functionally cohesive Python program (the core logic of the program is encapsulated in the YoloApril TagFusionNode class), designed to run as a node within the Robot Operating System (ROSNoetic) environment. Its internal structure, dependent libraries, and data interfaces are all clearly defined, as detailed in Tables 2 and 3 below:

[0054] Table 2

[0055]

[0056] Table 3

[0057]

[0058] For the input and output interfaces of the software program, the program nodes of this invention interact with the outside world through the standard ROS mechanism, and their input and output interfaces are clearly defined, as shown in Table 4 below:

[0059] Table 4 Input Interface

[0060]

[0061] Table 5 Output Interfaces

[0062]

[0063] The anti-blurring module includes a "locking unit" and a "measurement unit," which work together to calculate the pose in the blurred image.

[0064] (1) Locking Unit (Deep Learning Object Detection)

[0065] like Figure 5 As shown, a YOLOv8 pre-trained model is used as the deep learning object detection algorithm. It is loaded into the NVIDIA Jetson Orin Nano Super of the drone via the NVIDIA TensorRT inference engine, achieving an inference speed of 30fps, meeting real-time requirements. The specific execution logic of the locking unit is as follows:

[0066] Receive color image topics ( / camera / color / image_raw, sensor_msgs / Image format) published by a single integrated vision module.

[0067] Perform inference on each frame of the image and output the coordinates of the regions of interest (ROI) for two types of targets:

[0068] External target (golf ball): Output the pixel coordinates of the top left corner (u1,v1) and bottom right corner (u2,v2) of the ROI, with a confidence threshold of 0.7 (if the confidence threshold is lower than 0.7, it is determined that there is no target).

[0069] April Tag visual labels: Output the ROI coordinates of each of the two labels, with a confidence threshold of 0.6 (to ensure robust locking);

[0070] The ROI coordinates and corresponding target category (sphere / label 1 / label 2) are transferred to the measurement unit. If it is like... Figure 5 If there are 5 tags, then there are 3-5 more tags.

[0071] Example code is as follows:

[0072] import yolo_detector as yolo # Import the module interface that encapsulates the deep learning object detection algorithm

[0073] def lock_phase(full_color_image):

[0074] """

[0075] Locking phase: Perform object detection on the full-size image to obtain the region of interest.

[0076] enter:

[0077] full_color_image: The original full-size color image frame.

[0078] Output:

[0079] list_of_rois: A list of detected target bounding boxes, in the format [[x1,y1,x2,y2], ...].

[0080] """

[0081] # The main task of the object detection algorithm at this stage is to robustly provide the region of interest (ROI) of the target.

[0082] detected_rois = yolo.detect_objects(full_color_image)

[0083] return detected_rois

[0084] (2) Measurement unit (high-precision pose calculation)

[0085] The pyAprilTags library is used to implement April tag decoding and pose calculation algorithms, which are then combined with camera intrinsic parameters to complete the pose calculation.

[0086] Read camera intrinsics from the ROS parameter server (topic: / camera / color / camera_info, such as horizontal focal length f). x =920px, vertical focal length f y =920px, optical center coordinates c x =640px, c y =360px and distortion coefficient);

[0087] The ROI image block output by the locking unit is cropped (only the ROI area is retained, and background interference is removed);

[0088] For April Tag ROI: detect tag corner points (4 vertices), and use the PnP algorithm to calculate the tag's six-DOF pose in the camera coordinate system (translation vector T=(X,Y,Z), pose quaternion q=(x,y,z,w)).

[0089] For the ROI of a golf ball: Take the center pixel of the ROI ((u1+u2) / 2, (v1+v2) / 2), combine it with the corresponding pixel depth value Z output by the depth camera, and calculate the three-dimensional coordinates of the ball's center (X=(uc) / 2) using the camera projection model. x )*Z / f x Y=(vc y )*Z / f y (where Z is the depth value).

[0090] The calculated pose data (label 1 pose, label 2 pose, and sphere center coordinates) are transmitted to the multi-label pose fusion module.

[0091] Example code is as follows:

[0092] import apriltag_estimator as apriltag # Import the module interface that encapsulates the high-precision pose estimation algorithm

[0093] def measure_phase(full_gray_image, list_of_rois):

[0094] """

[0095] Measurement phase: Perform high-precision pose estimation within each region of interest (ROI).

[0096] enter:

[0097] full_gray_image: The original full-size grayscale image.

[0098] list_of_rois: A list of bounding boxes output by the locking phase.

[0099] Output:

[0100] list_of_poses: A list containing pose information for each successfully solved pose.

[0101] """

[0102] measured_poses = []

[0103] for roi in list_of_rois:

[0104] # Crops sub-images from the original image based on the ROI to improve the signal-to-noise ratio.

[0105] x1, y1, x2, y2 = roi

[0106] image_patch = full_gray_image[y1:y2, x1:x2]

[0107] try:

[0108] # Run the pose estimation algorithm on the sub-image.

[0109] # The algorithm improves the success rate and stability of solving sub-images with high signal-to-noise ratio.

[0110] pose = apriltag.estimate_pose(image_patch)

[0111] if pose:

[0112] measured_poses.append(pose)

[0113] except Exception:

[0114] # If the current ROI calculation fails, skip this step.

[0115] continue

[0116] return measured_poses

[0117] The multi-tag pose fusion module performs weighted fusion of pose data from multiple (e.g., two) visible April tags. The specific logic is as follows:

[0118] Set confidence weights: The weights are determined based on the physical size of the labels. For a 3cm×3cm label (label 1), the weight is w1=0.6, and for a 2cm×2cm label (label 2), the weight is w2=0.4 (the larger the size, the stronger the resistance to occlusion, and the higher the weight).

[0119] Translation vector fusion: calculated using a weighted average method, the formula is T. fused =(w1T1+w2T2) / (w1+w2), where T1 is the translation vector of label 1 and T2 is the translation vector of label 2; if there are three labels, the formula is T fused =(w1T1+w2T2+w3T3) / (w1+w2+w3), where T3 is the translation vector of label 3, w3 is the weight of label 3 (the remaining weights are readjusted), and w1+w2+w3=1 must be satisfied, and so on.

[0120] Attitude Quaternion Fusion:

[0121] Align hemispheres: Using the quaternion q1 of label 1 as a reference, calculate the dot product of the quaternion q2 of label 2 and q1 (q1・q2=x1x2+y1y2+z1z2+w1w2). If the dot product is less than 0, take the negative value of q2 (-q2) to ensure that the two are in the same reference hemisphere.

[0122] Weighted summation and normalization: calculation ,in Let w be the quaternion of the i-th label (label i) after alignment, N be the number of visible labels, and w be the number of visible labels. i Let be the weight of the i-th label (label i);

[0123] Output the fused end-effector pose (T) fused ,q fused ) to the time-domain smoothing module.

[0124] The code example is as follows:

[0125] import numpy as np

[0126] import quaternion_math as qm # Import the module interface that provides quaternion mathematical operations

[0127] def fusion_phase(list_of_raw_poses):

[0128] """

[0129] Fusion stage: Fusion of pose data from multiple labels with confidence weights.

[0130] enter:

[0131] list_of_raw_poses: A list whose elements are dictionaries containing poses and weights.

[0132] Format: [{'pose': T1, 'weight': 1.0}, ...]

[0133] Output:

[0134] fused_pose: A single, fused pose.

[0135] """

[0136] if not list_of_raw_poses: return None

[0137] # Extract position, pose, and weight data

[0138] translations = np.array([p['pose'].translation for p in list_of_raw_poses])

[0139] quaternions = np.array([p['pose'].quaternion for p in list_of_raw_poses])

[0140] weights = np.array([p['weight'] for p in list_of_raw_poses])

[0141] # Weighted average of position vectors

[0142] fused_translation = np.average(translations, axis=0, weights=weights)

[0143] # Weighted average of attitude quaternions

[0144] fused_quaternion = qm.weighted_average_quaternions(quaternions,weights)

[0145] return {'translation': fused_translation, 'quaternion': fused_quaternion}

[0146] The temporal smoothing module uses a temporal low-pass filtering algorithm to smooth the fused pose data and eliminate high-frequency noise (because even after fusion processing, the output pose sequence may still contain high-frequency noise caused by factors such as sensor noise and algorithm jitter).

[0147] Translation vector smoothing: An exponential moving average algorithm is used, with a smoothing factor α = 0.8 (determined based on the UAV vibration frequency (10-20Hz) and closed-loop control response requirements (0.1s)). The formula is T. smooth (k)=(1-α)T measured (k)+αT smooth (k-1), where k is the current frame, k-1 is the previous frame, and T mooth (k-1) is the smoothed translation vector from the previous frame, T measured(k) is the measurement translation vector of the current frame;

[0148] Attitude parameter smoothing: Spherical linear interpolation (Slerp) algorithm is used, with interpolation coefficients of (1-α)=0.2, and the formula is q. smooth (k)=Slerp (q smooth (k-1), q measured (k), 0.2), to ensure smooth attitude changes without jumps, where q smooth (k) represents the smoothed pose quaternion for the current frame, q measured (k) is the measurement pose quaternion for the current frame;

[0149] Output smoothed pose signal (T) smooth , q smooth (and the data interaction module with the center coordinates of the sphere.)

[0150] Example code is as follows:

[0151] class TemporalSmoother:

[0152] """

[0153] A time-domain filter is used to smooth pose data sequences.

[0154] """

[0155] def __init__(self, smoothing_factor=0.6):

[0156] self.alpha = smoothing_factor

[0157] self.last_smoothed_pose = None

[0158] def smooth(self, current_fused_pose):

[0159] """

[0160] Low-pass filtering is applied to the fused pose data of the new frame.

[0161] """

[0162] if self.last_smoothed_pose is None:

[0163] self.last_smoothed_pose = current_fused_pose

[0164] return current_fused_pose

[0165] # Apply Exponential Moving Average (EMA) filtering to the location component

[0166] current_pos = current_fused_pose['translation']

[0167] last_pos = self.last_smoothed_pose['translation']

[0168] smoothed_pos = self.alpha * last_pos + (1 - self.alpha) * current_pos

[0169] # Apply spherical linear interpolation (Slerp) to the attitude components (quaternions) to achieve a smooth transition.

[0170] current_quat = current_fused_pose['quaternion']

[0171] last_quat = self.last_smoothed_pose['quaternion']

[0172] smoothed_quat = qm.slerp(last_quat, current_quat, 1 - self.alpha)

[0173] smoothed_pose = {'translation': smoothed_pos, 'quaternion': smoothed_quat}

[0174] self.last_smoothed_pose = smoothed_pose # Update state

[0175] # The smoothed pose data is provided to the control system as the final output.

[0176] return smoothed_pose

[0177] The data interaction module uses the ROS standard communication mechanism to achieve data interaction with the external closed-loop control system.

[0178] Use a TF broadcaster to publish pose signals:

[0179] Broadcast the "gripper_center" coordinate system (corresponding to the center of the robotic arm's end effector gripper), with the pose being the smoothed (T) coordinate system. smooth , q smoot This information is used by the control system to obtain the position of the robotic arm's end effector.

[0180] Broadcast the "ball_closest" coordinate system (corresponding to the center of the nearest golf ball), with the pose being the smoothed three-dimensional coordinates of the ball's center, for the control system to make decisions on how to grasp the target;

[0181] The data transmission frequency is consistent with the camera frame rate (e.g., 30Hz) to ensure the real-time performance of closed-loop control.

[0182] Example 2

[0183] Based on the same concept, such as Figure 4-6 As shown, this invention also proposes a high-fidelity visual positioning method for state observation of high-vibration platforms, comprising the following steps:

[0184] S1. Deploy a single integrated vision module: Install a camera with color image acquisition and depth data acquisition functions on a high-vibration platform so that the camera's field of view simultaneously covers the external target and at least one visual tag on the end effector of the robotic arm.

[0185] In this embodiment, the Orbbec Gemini 335 camera is mounted on the bottom of the drone body, and the field of view is adjusted to cover the label at the end of the robotic arm and the external target object. The hardware wiring is completed (the camera's USB 3.0 is connected to the Jetson Orin NanoSuper, and the power is provided by the drone's battery).

[0186] S2. Anti-blurring processing: Locking and measurement operations are performed on the original video frames acquired by the single integrated vision module; the locking operation uses a deep learning target detection algorithm with strong generalization ability to robustly lock the region of interest of the external target and the visual label; the measurement operation uses a high-precision pose estimation algorithm to calculate the three-dimensional coordinates of the external target and the six-degree-of-freedom pose of the visual label based on the region of interest.

[0187] In this embodiment, the locking operation involves the YOLOv8 model inferring the original blurred image captured by the camera (such as edge blurring caused by drone vibration) to lock the ROI of the sphere and the label (both with confidence scores > 0.7, no missed detections). The measurement operation involves the pyAprilTags library calculating the ROI to obtain the pose of label 1 (T1=(0.2,0.1,1.5)m, q1=(0,0,0.3,0.95)), the pose of label 2 (T2=(0.21,0.12,1.51)m, q2=(0,0,0.32,0.948)), and the coordinates of the sphere center ((0.5,0.3,1.8)m). The labels here are not limited to two; they can be like... Figure 5 Five or more of the same type can be selected based on the actual situation. More labels mean a greater amount of computation and higher accuracy.

[0188] S3. Multi-label pose fusion: Obtain the pose data of all visible visual labels solved in step S2, and perform weighted fusion of the translation vector and pose parameters of the pose data according to the preset confidence weight to obtain fused pose data;

[0189] In this embodiment, the translation vector T is fused. fused =(0.6*(0.2,0.1,1.5)+0.4*(0.21,0.12,1.51)) / (0.6+0.4)=(0.204,0.108,1.504)m; Fusion attitude quaternion q fused =(0.6q1+0.4q2) / 0.6q1+0.4q2=(0,0,0.308,0.951).

[0190] S4. Temporal smoothing: A temporal low-pass filtering algorithm is used to process the fused pose data to remove high-frequency noise and data jitter, generating a smooth pose signal;

[0191] In this embodiment, if the smooth translation vector T of the previous frame smooth If (k-1)=(0.205,0.109,1.503)m, then the current T smooth (k)=(1-0.8)(0.204,0.108,1.504)+0.8(0.205,0.109,1.503)=(0.2048,0.1088,1.5032)m;

[0192] posture q smooth (k)=Slerp(q smooth (k-1),q fused ,0.2), to obtain a smooth pose (0,0,0.307,0.951).

[0193] S5. Data Output: Transmits smooth pose signals to the external closed-loop control system of the high-vibration platform, providing high-fidelity state observation data for the external closed-loop control system.

[0194] In this embodiment, the TF broadcaster publishes the coordinate system of "gripper_center" and "ball_closest". After receiving the coordinates, the control system generates a grasping command to drive the robotic arm to move.

[0195] This improves the control precision of the robotic arm from 3cm to 3mm (because the positioning error of the end effector measured by the laser rangefinder is less than 3mm in multiple tests), which can fully meet the requirements of dynamic grasping; when the drone's vibration frequency is 15Hz, the success rate of directly detecting the April Tag is less than 10%, while the success rate of the "lock-measure" process of this system is greater than 92%; the smoothed pose signal has no high-frequency jitter, and the robotic arm's movement trajectory is smooth and without tremors (observed by a high-speed camera, the maximum jitter amplitude of the end effector is less than 0.5mm); the success rate of dynamically grasping a golf ball has been improved from less than 20% in the existing technology to more than 90% (46 successful out of 50 tests).

[0196] Example 3

[0197] This embodiment also provides a readable storage medium storing a computer program, the computer program including program code for controlling a process to execute the process, the process including the high-fidelity state observation visual positioning method for a high-vibration platform according to Embodiment 2.

[0198] It should be noted that the specific examples in this embodiment can refer to the examples described in the above embodiments and optional implementations, and will not be repeated here.

[0199] Generally, various embodiments can be implemented in hardware or dedicated circuitry, software, logic, or any combination thereof. Some aspects of the invention can be implemented in hardware, while others can be implemented by firmware or software executed by a controller, microprocessor, or other computing device, but the invention is not limited thereto. Although various aspects of the invention may be shown and described as block diagrams, flowcharts, or using some other graphical representation, it should be understood that, by way of non-limiting example, these blocks, apparatuses, systems, techniques, or methods described herein can be implemented in hardware, software, firmware, dedicated circuitry or logic, general-purpose hardware or controllers or other computing devices, or some combination thereof.

[0200] Embodiments of the present invention can be implemented by computer software, which may be executable by a data processor of a mobile device, such as a processor entity, or by hardware, or by a combination of software and hardware. Computer software or programs (also referred to as program products) including software routines, applets, and / or macros can be stored in any device-readable data storage medium, and they include program instructions for performing specific tasks. The computer program product may include one or more computer-executable components configured to perform the embodiments when the program is run. The one or more computer-executable components may be at least one piece of software code or a portion thereof. Additionally, it should be noted in this respect that, as Figure 2 Any box in the logical flow can represent a program step, or interconnected logic circuits, boxes and functions, or a combination of program steps and logic circuits, boxes and functions. Software can be stored on physical media such as memory chips or blocks of storage implemented within a processor, magnetic media such as hard disks or floppy disks, and optical media such as DVDs and their data variants, CDs, etc. The physical medium is a non-transient medium.

[0201] Those skilled in the art should understand that the technical features of the above embodiments can be combined in any way. For the sake of brevity, not all possible combinations of the technical features in the above embodiments have been described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.

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

Claims

1. A high-fidelity state-observation vision positioning system for a high-vibration platform, characterized by, Comprise: A single integrated vision module, a camera with color image acquisition and depth data acquisition functions, its installation position and field of view range are configured to be able to cover at least one visual label mounted on the end effector of the robot on the high-vibration platform and the external target object at the same time; An anti-blurring processing module, including a locking unit and a measurement unit; The locking unit is configured to use a deep learning target detection algorithm to process the original video frames collected by the single integrated vision module, and robustly lock the respective interest regions of the external target object and the visual label; The measurement unit is configured to use a high-precision pose estimation algorithm to process the interest region output by the locking unit, and solve the three-dimensional coordinates of the external target object and the six-degree-of-freedom pose of the visual label; A multi-label pose fusion module configured to obtain the pose data of all visible visual labels solved by the measurement unit, and respectively weight and fuse the translation vector and attitude parameter of the pose data according to the preset confidence weight, and output the fused pose data; A time domain smoothing module configured to use a time domain low-pass filtering algorithm to process the fused pose data output by the multi-label pose fusion module, filter out high-frequency noise and data jitter, and obtain a smooth pose signal; A data interaction module configured to transmit the smooth pose signal output by the time domain smoothing module to the external closed-loop control system to provide state observation feedback for the external closed-loop control system; Wherein, the confidence weight of the multi-label pose fusion module is determined according to the physical size of the visual label, and the larger the physical size of the visual label, the higher the confidence weight corresponding to the attitude parameter, the weighting fusion of the attitude parameter includes aligning the attitude quaternions of all visual labels to the same reference hemisphere, and then performing weighted summation and normalization processing.

2. The high-fidelity state observation vision positioning system for a high vibration platform of claim 1, wherein, The single integrated vision module is installed below the fuselage of the high-vibration platform, and the high-vibration platform is a UAV carrying the robot.

3. The high fidelity state observation vision positioning system for a high vibration platform of claim 1, wherein, The deep learning target detection algorithm used by the locking unit is a YOLO series target detection algorithm, which loads a pre-trained model through a special inference engine and performs image inference to output the coordinate range of the interest region.

4. The high-fidelity state observation vision positioning system for a high vibration platform of claim 1, wherein, The high-precision pose estimation algorithm used by the measurement unit is an April Tag label decoding and pose solving algorithm, which combines the camera intrinsic distortion coefficients of the single integrated vision module to solve the six-degree-of-freedom pose of the visual label in the coordinate system of the single integrated vision module.

5. A high-fidelity state observation vision positioning system for a high vibration platform as claimed in any one of claims 1 to 4, characterized in that, The time domain smoothing module uses an exponential moving average algorithm to filter the translation vector in the fused pose data, and uses a spherical linear interpolation algorithm to filter the attitude parameter in the fused pose data.

6. A high fidelity state observation visual positioning method for high vibration platforms, characterized in that, The steps include: S1. Deploy a single integrated vision module: install a camera with color image acquisition and depth data acquisition functions on a high-vibration platform, so that the field of view of the camera covers at least one visual label on the end effector of the robot and the external target object at the same time; S2. Anti-blurring processing: performing a locking operation and a measurement operation on the original video frames collected by the single integrated visual module; the locking operation is to robustly lock the region of interest of the external target and the visual label by using a deep learning target detection algorithm with strong generalization ability; the measurement operation is to obtain the three-dimensional coordinates of the external target and the six-degree-of-freedom pose of the visual label based on the region of interest by using a high-precision pose estimation algorithm; S3. Multi-label pose fusion: obtaining the pose data of all visible visual labels calculated in step S2, and performing weighted fusion on the translation vector and the attitude parameter of the pose data according to a preset confidence weight, to obtain fused pose data; wherein the preset confidence weight is positively correlated with the physical size of the visual label, and the larger the physical size of the visual label, the higher the confidence weight; the weighted fusion of the attitude parameter includes first judging the sign consistency of the attitude quaternion of each visual label and a reference quaternion, aligning to the same hemisphere, and then performing weighted calculation; S4. Time domain smoothing processing: processing the fused pose data by using a time domain low-pass filtering algorithm to filter out high-frequency noise and data jitter, and generating a smoothed pose signal; S5. Data output: transmitting the smoothed pose signal to an external closed-loop control system of a high-vibration platform, and providing high-fidelity state observation data for the external closed-loop control system.

7. The high fidelity state observation visual positioning method for high vibration platforms of claim 6, wherein, The deep learning target detection algorithm in step S2 is a YOLO series algorithm, which outputs the pixel coordinate range of the region of interest by loading a pre-trained model and performing inference on special inference hardware.

8. The high fidelity state observation visual positioning method for high vibration platforms as claimed in claim 6, wherein, The high-precision pose estimation algorithm in step S2 is an April Tag algorithm, which combines the intrinsic data of the single integrated visual module to perform corner detection and decoding on the visual label in the region of interest, and calculates the six-degree-of-freedom pose of the visual label.

Images