Vision-guided robot pick-and-place precision positioning method and system

Abstract

The present application relates to the technical field of robot vision positioning, and discloses a robot feeding and discharging precision positioning method and system based on visual guidance, which comprises the following steps: acquiring a global depth image and extracting initial three-dimensional coordinates, guiding a local three-dimensional camera to an initial observation pose; collecting local point clouds, calculating point cloud quality indexes and kinematic operability to construct a joint cost function; when the set threshold condition is not met, synthesizing a joint gradient vector to drive the mechanical arm to detect; after the set threshold condition is met, extracting multiple frames of local point clouds and mapping them to a base coordinate system to generate global fusion point clouds; registering the global fusion point clouds with template point clouds to solve a pose transformation matrix and generate a feeding and discharging operation path. The present application comprehensively considers the visual imaging quality and the underlying kinematic state to drive the mechanical arm to perform micro-motion optimization, effectively avoids the mechanical arm from falling into a singular configuration, overcomes the data missing defects caused by workpiece reflection and the like, and improves the success rate of positioning and grabbing under complex working conditions.

Description

Technical Field

[0001] This invention relates to the field of robot vision positioning technology, specifically to a vision-guided robot loading and unloading precision positioning method and system. Background Technology

[0002] In automated loading and unloading operations in industrial production, target workpieces are often stacked or randomly placed. To ensure that the end effector of the robotic arm can accurately grasp the workpieces, avoid physical collisions, and meet the assembly tolerance requirements of subsequent processes, machine vision technology is widely used in the industry to guide the movement trajectory and compensate for the posture of the robotic arm.

[0003] To improve the positioning accuracy of robots loading and unloading materials, the industry has conducted a great deal of technological research and development. For example, patent application CN112589787A discloses a visual positioning and hand-eye calibration method for loading and unloading samples by a robotic arm on a feeding turntable. This method completes system calibration and calculates the target pose by setting visual markers at the end of the robotic arm and on the workpiece. Building upon this, for dynamic operation scenarios, patent application CN112456128B discloses an automatic loading and unloading method and system on an assembly line based on machine vision. This method combines workpiece position information with the conveyor line's movement speed to calculate the gripping position to compensate for the assembly line's cycle time. Furthermore, to obtain depth spatial information, patent application CN106625676A discloses a three-dimensional vision-based precise guidance and positioning method for automatic loading in automotive intelligent manufacturing. This method uses three-dimensional vision to obtain the posture of parts and calculates deviations to correct the robotic arm's movement trajectory. Simultaneously, in terms of point cloud data processing, patent application CN119873310A discloses a 3D vision-based method for registering the position of a loading platform. This method calculates pose deviations by matching the target point cloud with the template point cloud to compensate for position deviations. These existing technologies improve the automation level of material handling operations from dimensions such as identification-assisted calibration, dynamic cycle compensation, three-dimensional trajectory correction, and point cloud registration.

[0004] However, the aforementioned visual-guided positioning schemes still have technical limitations when dealing with complex industrial site conditions. Existing visual positioning methods generally rely on ideal surface imaging conditions and preset fixed observation angles. When the target workpiece surface has metallic reflective properties or is in a partially occluded state, static 3D imaging from a single fixed angle often produces large areas of data holes and distortion noise, leading to subsequent point cloud feature registration failure or a severe decrease in accuracy. To obtain complete workpiece surface data, directly controlling a local 3D camera to take multi-angle shots is a common method, but existing technologies only focus on the coverage of visual features in one direction when planning the camera's movement and detection trajectory, without taking into account the kinematic physical constraints of the robot arm's underlying structure. In actual operation, this one-way control logic is very likely to cause the robot arm to encounter kinematic singular configurations when approaching the edge of the hopper or adjusting complex postures, causing the underlying joints to lose motion margin, which in turn leads to trajectory tracking failure or equipment alarm shutdown. Currently, the industry lacks a precise positioning method that can simultaneously consider the quality of 3D point cloud imaging and the flexibility of the robot's underlying physical pose, and autonomously generate micro-motion trajectories to obtain global fusion data. Summary of the Invention

[0005] To address the shortcomings of existing technologies, this invention provides a vision-guided method and system for precise positioning of robot loading and unloading, which solves the problems of missing visual point cloud data due to workpiece surface reflection during robot loading and unloading, and the problem of the robotic arm easily getting stuck in singular configurations when searching for the best observation angle.

[0006] To achieve the above objectives, the present invention provides the following technical solution: The first aspect of the invention provides a vision-guided method for precise positioning during robot loading and unloading, comprising the following steps: Acquire global depth images and extract the initial 3D coordinates of the target workpiece to guide the local 3D camera to the initial observation pose; The local 3D camera is used to collect local point cloud quality index data, and the kinematic maneuverability is calculated based on the Jacobian matrix of the robotic arm. A joint cost function is constructed based on the point cloud quality index and the kinematic operability; based on the joint cost function, it is determined whether the current pose meets a set threshold condition. If not satisfied, a joint gradient vector is synthesized based on the point cloud quality index and the kinematic maneuverability, driving the robotic arm to perform micro-motion detection and continuously collect local point clouds until the set threshold condition is met. After the set threshold condition is met, multiple frames of the local point cloud are extracted and mapped to the robot base coordinate system to generate a global fused point cloud; The global fused point cloud is registered with the template point cloud of the target workpiece, and the pose transformation matrix is ​​calculated to drive the robotic arm to complete the loading and unloading operations.

[0007] Preferably, the step of synthesizing a joint gradient vector based on the point cloud quality index and the kinematic operability includes: Calculate the visual gradient vector of the point cloud quality index and the kinematic gradient vector of the kinematic operability, respectively. The kinematic gradient vector is mapped from the robot's joint space to Cartesian space; The kinematic gradient vector mapped to Cartesian space is combined with the visual gradient vector to obtain the joint gradient vector used to issue micro-motion commands.

[0008] Preferably, the extraction of the initial three-dimensional coordinates of the target workpiece includes: A denoising and filtering operation is performed on the global depth image to obtain a denoised global target point cloud. Euclidean clustering algorithm is used to divide data points whose distance is less than a set clustering threshold into the same workpiece cluster set. Select the set with the largest number of point clouds as the target point cloud cluster, and calculate the eigenvalues ​​and eigenvectors of the covariance matrix of the target point cloud cluster. Based on the feature values ​​and feature vectors, a directed bounding box is constructed that tightly surrounds the target point cloud cluster. The geometric centroid of the directed bounding box is extracted, and the geometric centroid is determined as the initial three-dimensional coordinates of the target workpiece.

[0009] Preferably, the calculation of the point cloud quality index specifically involves constructing a scalar function that integrates the proportion of spatial integrity of the point cloud with the degree of distortion of the surface normal vector, and calculating it according to the following formula: ; In the formula, , The ideal total number of point clouds within the region of interest is a preset constant. This represents the number of effective 3D points actually captured in a single frame of local point cloud. This is defined as the total number of neighborhood points within the search radius of a local point cloud in a single frame. For the neighboring region Local normal vectors of valid 3D points The average normal vector within the local neighborhood. and These are the first normalized weight constant and the second normalized weight constant, respectively. For the natural constant An exponential function with base 0. The L2 norm of a vector is used to represent the vector's L2 norm.

[0010] Preferably, the step of constructing a joint cost function and determining whether the current pose meets a set threshold condition based on the joint cost function specifically includes: The value of the joint cost function is calculated by fusing the point cloud quality index and the kinematic operability using a linear weighted combination method. When the value of the joint cost function is not greater than the preset lower limit threshold of the cost function, it is determined that the preset threshold condition is not met.

[0011] Preferably, calculating the visual gradient vector of the point cloud quality index includes: A quadratic polynomial response surface model is constructed in the local Cartesian space to fit the mapping relationship between the local position deviation vector recorded in the sliding window and the point cloud quality index. Substitute the data nodes in the sliding window into the quadratic polynomial response surface model to construct an overdetermined system of equations, call the least squares algorithm to perform matrix pseudo-inverse operations, and solve for the optimal solutions of constant term coefficients, first-order coefficient vectors, and second-order symmetric coefficient matrices. The first-order partial derivative of the fitted quadratic polynomial response surface model is solved at the local position deviation vector where the vector is zero, and the extracted first-order coefficient vector is used as the visual gradient vector of the point cloud quality index.

[0012] Preferably, the step of calculating the kinematic gradient vector of the kinematic operability, mapping the kinematic gradient vector to Cartesian space, and then synthesizing a joint gradient vector with the visual gradient vector includes: The kinematic maneuverability is analytically differentiated, and the partial derivatives with respect to the angles of each joint of the robotic arm are calculated to generate the joint space kinematic gradient vector defined in the joint space. Call the transpose of the pseudo-inverse of the Jacobian matrix under the current configuration, map the joint space kinematic gradient vector to Cartesian space, and generate a kinematic gradient vector mapped to Cartesian space. The fitted visual gradient vector and the kinematic gradient vector mapped to Cartesian space are linearly weighted and synthesized to generate the joint gradient vector used for issuing drive control.

[0013] Preferably, the method of driving the robotic arm to perform micro-motion detection includes: Set a proportional gain coefficient, multiply the joint gradient vector by the proportional gain coefficient, and generate the Cartesian space end velocity command; When the velocity modulus of the Cartesian space end velocity command exceeds the preset maximum safe linear velocity or maximum safe angular velocity, the velocity component is scaled proportionally to perform amplitude limiting filtering. The Cartesian space end velocity command, after being filtered and limited, is substituted into the inverse mapping relationship of the Jacobian matrix to calculate the underlying joint space velocity command and send it to the underlying servo driver to drive the robotic arm to perform adaptive micro-motion trajectory deformation.

[0014] Preferably, the extraction of the local point cloud from multiple frames includes: Set a time comparison window on the time axis and iterate through the values ​​of the joint cost function during the micro-motion detection process; When the value of the joint cost function in any sampling period is the maximum function value within the time comparison window, the pose of the corresponding timestamp is determined to be a local extremum point. Extract the local point cloud corresponding to the local extreme point in a single frame, and store each extracted local point cloud in a valid data queue to form the local point cloud in multiple frames.

[0015] Preferably, the step of mapping to the robot base coordinate system to generate a global fused point cloud includes: Call the pre-calibrated hand-eye transformation matrix to calculate the spatial coordinate system transformation matrix of the robotic arm end flange coordinate system relative to the robot base coordinate system at the moment of local point cloud acquisition in each frame; The physical space mapping operation is performed on the local point cloud of the multiple frames using the spatial coordinate system transformation matrix, so as to unify the local point cloud of the multiple frames to the robot base coordinate system; A three-dimensional voxel mesh is constructed in the robot's base coordinate system. The three-dimensional coordinate centroid of overlapping data points within the micro cube is calculated. The three-dimensional coordinate centroid is used to replace the overlapping data points within the micro cube. Downsampling and deduplication processing is performed to generate the global fused point cloud.

[0016] Preferably, the step of registering the globally fused point cloud with the template point cloud of the target workpiece and solving the pose transformation matrix includes: Extract the fast point feature histograms of the template point cloud and the global fused point cloud, and perform coarse registration to generate an initial spatial transformation matrix; The global fused point cloud is set as the target point set, and the template point cloud adjusted by the initial spatial transformation matrix is ​​set as the source point set. A spatial distance objective function containing a three-dimensional rotation matrix and a three-dimensional translation vector is constructed to execute the iterative nearest point algorithm. Extract the three-dimensional rotation matrix and three-dimensional translation vector in the convergent state, and assemble them into the pose transformation matrix.

[0017] Preferably, generating the loading and unloading operation path according to the pose transformation matrix includes: calculating the root mean square error between the corresponding feature points of the transformed template point cloud and the global fused point cloud using the pose transformation matrix; When the root mean square error is less than the preset physical assembly tolerance threshold, the absolute target pose of the target workpiece gripping point is calculated according to the pose transformation matrix, and a safe approach point is generated by shifting outward in the normal direction. A three-dimensional operation path is planned and generated using a polynomial interpolation algorithm, and the three-dimensional operation path is determined as the loading and unloading operation path.

[0018] The second aspect of the present invention provides a vision-guided robot loading and unloading precision positioning system for implementing the positioning method described in the first aspect of the present invention, including a robotic arm, a global 3D camera, a local 3D camera, and a control system; The global 3D camera is fixedly mounted above the working area to acquire global depth images; The local 3D camera is mounted on the end effector flange of the robotic arm and is used to acquire local point clouds of the target workpiece. The control system is equipped with: The vision guidance module is used to acquire global depth images and extract the initial 3D coordinates of the target workpiece, and guide the local 3D camera to the initial observation pose; The data evaluation module is used to collect local point cloud quality indexes using the local 3D camera and calculate kinematic maneuverability based on the Jacobian matrix of the robotic arm. The optimization control module is used to construct a joint cost function based on the point cloud quality index and the kinematic maneuverability; and to determine whether the current pose meets the set threshold condition based on the joint cost function. If it does not meet the threshold condition, a joint gradient vector is synthesized based on the point cloud quality index and the kinematic maneuverability to drive the robotic arm to perform micro-motion detection and continuously collect local point clouds until the set threshold condition is met. The pose analysis module is used to extract multiple frames of local point cloud and map them to the robot base coordinate system to generate a global fused point cloud after the set threshold conditions are met; the global fused point cloud is registered with the template point cloud of the target workpiece and the pose transformation matrix is ​​calculated to drive the robotic arm to complete the loading and unloading operations.

[0019] A third aspect of the present invention provides a vision-guided robot loading and unloading precision positioning system, comprising a robotic arm, a global 3D camera, a local 3D camera, and a control system; the global 3D camera is fixedly mounted above the work area for acquiring a global depth image; the local 3D camera is mounted at the end effector flange of the robotic arm for acquiring a local point cloud of the target workpiece; the control system is used to execute any one of the vision-guided robot loading and unloading precision positioning methods described in the first aspect.

[0020] A fourth aspect of the present invention provides an electronic device, including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the computer program to implement the vision-guided robot loading and unloading precision positioning method described in the first aspect of the present invention.

[0021] The fifth aspect of the present invention provides a computer-readable storage medium storing a computer program that, when executed by a processor, implements the vision-guided robot loading and unloading precision positioning method described in the first aspect of the present invention.

[0022] The vision-guided robot loading and unloading precision positioning method and system provided by this invention have the following beneficial effects: This invention constructs a joint cost function based on the point cloud quality index and kinematic maneuverability, and synthesizes a joint gradient vector to drive the robotic arm to perform micro-motion detection when the set threshold condition is not met. Because it comprehensively considers the visual imaging quality and the robot's underlying kinematic state, the system can autonomously drive the local 3D camera to a position that can both collect the complete point cloud and maintain the flexibility of the robotic arm's posture when it is affected by the reflection interference of the workpiece surface or the limited local observation angle. This avoids the robotic arm getting stuck in a kinematic singular configuration during the optimization process, and improves the success rate and operational stability of loading and unloading positioning under complex working conditions.

[0023] This invention extracts local point clouds corresponding to local extreme points from multiple frames, uses a spatial coordinate system transformation matrix to physically map them to the robot's base coordinate system to generate a global fused point cloud, and performs downsampling and deduplication processing under a three-dimensional voxel mesh. This overcomes the defects of reflective holes and missing data that are easily generated by single-view shooting. By stacking multi-view data, the complete surface geometric features of the workpiece are obtained. At the same time, the three-dimensional coordinate centroid is used to replace and remove overlapping data. Without losing the feature registration accuracy, the amount of redundant data is effectively reduced, the computational load of the control system is reduced, and the processing cycle of subsequent pose calculation is shortened.

[0024] This invention extracts the initial 3D coordinates using a global 3D camera, guides a local 3D camera to the initial observation pose, and performs feature registration between the generated global fused point cloud and the template point cloud to calculate the pose transformation matrix. This allows for the planning of loading and unloading operation paths. On one hand, it combines the advantages of global large-field collision-free search and local high-precision observation. On the other hand, it generates a safe approach point by offsetting outwards in the direction of the absolute target pose normal, which can eliminate the accumulated tolerances of physical assembly and reduce the risk of collision at the end of the robotic arm, ensuring the accuracy and safety of the target workpiece gripping operation. Attached Figure Description

[0025] Figure 1 This is a schematic diagram of the architecture of a vision-guided robot loading and unloading precision positioning system provided by the present invention. Figure 2 This is a flowchart of a vision-guided robot loading and unloading precision positioning method provided by the present invention; Figure 3 This is a schematic diagram of the sub-process of the multi-source state real-time assessment and index calculation operation provided by the present invention; Figure 4 This is a schematic diagram of the sub-process of optimization triggering and gradient estimation operation based on joint cost function and sliding response surface provided by the present invention; Figure 5 This is a schematic diagram of the sub-process of multi-source heterogeneous gradient synthesis and underlying physical closed-loop control operation provided by the present invention; Figure 6 This is a schematic diagram of a sub-process of the multi-view data fusion and spatial unified mapping operation provided by the present invention. Detailed Implementation

[0026] The technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0027] Example 1: See Figure 1 The present invention provides a vision-guided robot loading and unloading precision positioning system, which may include: a robotic arm, a global 3D camera, a local 3D camera, and a control system.

[0028] In the embodiments of this invention, the robotic arm, which serves as the physical actuator, is a six-degree-of-freedom industrial robotic arm with an external axis communication interface for receiving low-level motion control commands from the control system.

[0029] A global 3D camera is fixedly mounted above the work area and communicates with the control system to acquire global depth images of the work area. A local 3D camera is mounted on the end effector flange of the robotic arm and communicates with the control system to acquire local point clouds of the target workpiece while the robotic arm is in motion. The control system establishes data communication connections with the robotic arm, the global 3D camera, and the local 3D camera, respectively.

[0030] The control system is equipped with multiple functional modules to perform data processing and equipment scheduling. The system's logical architecture may include a vision guidance module, a data evaluation module, an optimization control module, and a pose analysis module.

[0031] The vision guidance module is used to control the global 3D camera to acquire images and analyze the initial 3D coordinates of the target workpiece, which is the stacked target workpiece to be loaded and unloaded in the work area.

[0032] The data evaluation module is used to simultaneously calculate the point cloud quality index under the current field of view of the local 3D camera and to calculate the real-time kinematic maneuverability of the robotic arm.

[0033] The optimization control module is used to construct a joint cost function based on the point cloud quality index and kinematic operability, and generate micro-motion adjustment commands by calculating gradient vectors and sending them to the underlying controller of the robotic arm.

[0034] The pose analysis module is used to perform spatial mapping and feature registration on multi-frame local point clouds, and generate a precise positioning pose transformation matrix for the target workpiece to drive the robot end effector to complete loading and unloading actions.

[0035] This embodiment effectively overcomes the technical problems of insufficient positioning accuracy and missing point clouds caused by the susceptibility of traditional single-view 3D cameras to workpiece occlusion and reflection in the background technology by combining global visual coarse localization with local multi-frame point cloud fusion technology. On the other hand, it jointly evaluates the cost of visually perceived point cloud quality and optimizes the kinematic maneuverability of the robotic arm itself through micro-motion. This not only ensures that the local camera can acquire the highest quality feature data, but also ensures that the robotic arm is in a highly flexible posture during observation and subsequent grasping. It solves the industry pain point that the robotic arm was trapped in singularities or exceeded the joint limits and could not perform loading and unloading actions due to the pursuit of blind spots in visual observation. This improves the success rate of precise loading and unloading of robots and the stability of system operation in complex stacking conditions.

[0036] Example 2: See Figure 2 This invention provides a vision-guided robot loading and unloading precision positioning method, comprising the following steps: S100 acquires the global depth image of the work area through a global 3D camera, extracts the initial 3D coordinates of the target workpiece from the global depth image, and guides the robotic arm, which acts as the robot's actuator, to the initial observation pose facing the target workpiece based on vision guidance.

[0037] The S200, based on the robot's motion state after being guided to the initial observation pose by vision, continuously acquires the local point cloud of the target workpiece using a local 3D camera, calculates the point cloud quality index in real time under the current observation pose, and simultaneously reads the current joint angle vector of the robotic arm to calculate the kinematic maneuverability.

[0038] S300 constructs a joint cost function based on the calculated point cloud quality index and kinematic maneuverability. When the joint cost function value does not meet the set threshold condition, a sliding window is established to cache historical data to fit the visual gradient vector of the point cloud quality index. At the same time, the kinematic gradient vector of the kinematic maneuverability is calculated, and the joint gradient vector is synthesized to drive the robotic arm to perform trajectory micro-motion detection for precise positioning until the set threshold condition is met.

[0039] S400: When the set threshold condition is met, extract multiple frames of local point clouds in the local extreme state of the joint cost function during the micro-motion detection process, and use the forward kinematics model to uniformly map the multiple frames of local point clouds to the robot base coordinate system to generate a global fused point cloud.

[0040] The S500 uses an iterative nearest point algorithm to perform feature registration between the global fused point cloud and the template point cloud of the target workpiece to calculate the pose transformation matrix. Based on the pose transformation matrix, it generates loading and unloading operation paths and drives the robotic arm's closed end effector to complete the precise positioning and loading / unloading of the target workpiece.

[0041] In this embodiment, step S100 mainly performs global guidance and initial pose localization operations to provide a physical space starting point for subsequent local detection, specifically including the following sub-steps: S101, the control system receives the work cycle start signal from the external workstation and sends an image acquisition trigger command to the global 3D camera via the industrial Ethernet communication protocol. Upon receiving the trigger command, the global 3D camera performs a single-exposure imaging of the hopper and the workpieces stacked inside it within the work area, acquiring a global depth image containing environmental depth information. Because ambient light interference and the physical characteristics of the sensor components can cause noise in the global depth image, the control system performs noise reduction filtering on the global depth image.

[0042] In this embodiment, the control system uses a pass-through filter to set a depth threshold range to remove invalid background data located outside the material box boundary. The depth threshold range is set according to the actual physical height range of the material box on the worktable. The control system extracts data from inside the material box and converts it into a point cloud format. A statistical outlier filter is used to calculate the spatial distance distribution characteristics within the local point cloud neighborhood, removing isolated noise points that deviate from the standard deviation range to obtain a denoised global target point cloud.

[0043] S102, the control system performs spatial segmentation on the denoised global target point cloud and extracts the initial three-dimensional coordinates of the target workpiece. In this embodiment, the control system uses a Euclidean clustering algorithm to evaluate the spatial Euclidean distance between each data point in the point cloud, and divides data points with a distance less than a set clustering threshold into the same workpiece cluster set.

[0044] Specifically, the implementation process of the Euclidean clustering algorithm includes: First, constructing a Kd-Tree (K-dimensional tree) data structure based on the denoised global target point cloud to accelerate spatial neighborhood search; second, randomly selecting an unprocessed seed point from the point cloud and placing it into the current clustering queue; then, using the Kd-Tree to search for the nearest neighbor of the seed point within a set clustering threshold (i.e., the search radius parameter, calibrated based on the minimum physical size characteristics of the target workpiece to be grasped). If the nearest neighbor is not processed, it is added to the current clustering queue; traversing all data points in the current clustering queue and performing the above nearest neighbor search operation until the number of points in the clustering queue no longer increases, thus forming an independent workpiece cluster set; finally, reselecting a seed point from the remaining unprocessed point cloud and repeating the above process until all data points have been traversed and divided. This algorithm segments based on the actual physical connectivity between points, without needing to pre-set the total number of clusters, and can effectively separate the scattered point clouds of individual independent workpieces stacked disorderly in the material bin.

[0045] The control system calculates the number of point clouds in each workpiece cluster and selects the cluster with the largest number of point clouds or the cluster with the highest spatial position (i.e., the largest Z-axis coordinate) as the current target point cloud cluster to be captured. For the selected target point cloud cluster, the control system uses principal component analysis to calculate the eigenvalues ​​and eigenvectors of the covariance matrix of the target point cloud cluster, determines the main distribution direction of the point cloud data in space, and constructs a directed bounding box that tightly surrounds the target point cloud cluster.

[0046] The specific implementation steps include: Calculate the three-dimensional centroid coordinates of the target point cloud cluster; The centroid coordinates are subtracted from the coordinates of each data point in the target point cloud cluster to perform decentralization processing; A 3×3 covariance matrix is ​​constructed using the decentralized point cloud data; Perform singular value decomposition (SVD) or eigenvalue decomposition on the covariance matrix to obtain three mutually orthogonal eigenvectors and their corresponding eigenvalues; The three eigenvectors obtained are used as the X-axis, Y-axis and Z-axis directions of the local spatial coordinate system, which represent the longest, second longest and shortest principal component directions of the point cloud data in three-dimensional space. Projecting all data points of the target point cloud cluster onto the directions of these three feature vectors, and finding the maximum and minimum values ​​in each direction, the length, width, and height boundary dimensions of the directed bounding box are determined, thus completing the construction of the bounding box. The control system extracts the geometric centroid of the directed bounding box and uses it as the initial three-dimensional coordinates of the target workpiece in the global camera reference frame.

[0047] S103, the control system calculates the inverse kinematics target value of the robotic arm based on the initial three-dimensional coordinates of the target workpiece, and issues motion commands to execute collision-free guidance. In this embodiment, the control system calls the pre-calibrated camera extrinsic parameter matrix, which is generated by the global hand-eye calibration process during the system initialization phase, mapping the initial three-dimensional coordinates from the coordinate space of the global three-dimensional camera to the physical base coordinate system of the robotic arm. The spatial mapping process is calculated using the following homogeneous transformation formula: ; In the formula, The initial three-dimensional coordinates of the target workpiece in the global three-dimensional camera coordinate system; Let be the homogeneous transformation matrix of the external parameters from the global 3D camera coordinate system to the robot arm base coordinate system; To transform the target workpiece's three-dimensional coordinates to the base coordinate system. To satisfy the matrix multiplication rules, and All are represented by four-dimensional homogeneous column vectors, that is, a constant 1 is added to the end of the three-dimensional coordinate data.

[0048] From a physical perspective, spatial mapping essentially converts the relative distance parameters of the target workpiece observed from the global camera viewpoint into a physical spatial position with the robot arm base as the absolute reference center, thereby enabling the control system to clearly determine the spatial target point that the robot arm should go to in the real physical world.

[0049] Get Subsequently, to prevent the local 3D camera mounted at the end of the robotic arm from physically colliding with the hopper or workpiece as it approaches downwards, the control system... The initial observation pose in Cartesian space is synthesized by superimposing a preset safety offset distance onto the positive Z-axis direction. This safety offset distance is set based on the optimal imaging focal length and working field of view of the local 3D camera, combined with the system's default downward tilting posture parameters. The control system then inputs the initial observation pose into the inverse kinematics model of the robotic arm to solve for the corresponding target values ​​of the six joint angles.

[0050] The inverse kinematics model described in this embodiment is a classic mathematical analytical model based on existing common knowledge, rather than an artificial intelligence neural network model. In specific implementation, this model is constructed based on the standard Denavit-Hartenberg (DH) parametric method of the robotic arm body, using a homogeneous transformation matrix to describe the spatial geometric pose relationship between adjacent links. The control system takes the synthesized initial observed pose in Cartesian space (including a three-dimensional position vector and a three-dimensional attitude rotation matrix) as the model input, substitutes it into the inverse kinematics equations, and uses an algebraic analytical method or a numerical iterative method to solve for the target rotation angle values ​​required for each joint of the robotic arm.

[0051] In this embodiment, the control system loads a simplified geometric bounding sphere model containing the dimensions of the robotic arm's main link and the worktable boundary in memory, and performs collision interference verification on the calculated joint angle target values.

[0052] The simplified geometric enclosing sphere model is a spatial geometric approximation for rapid collision detection. Specifically, it is defined as follows: a series of virtual spheres with known radii completely enclose the links of the robotic arm and the end-effector local camera. This model defines the relative coordinates of the center of each enclosing sphere and its radius in the link's local coordinate system. During collision interference verification, the control system, combined with the calculated joint angle target values, derives the absolute spatial coordinates of the center of each enclosing sphere in the current base coordinate system using forward kinematics. Then, it calculates the shortest Euclidean distance from the center of each enclosing sphere to the worktable boundary or the surface of a known obstacle. If any calculated shortest Euclidean distance is less than or equal to the radius of the enclosing sphere itself, a collision interference is determined to have occurred; otherwise, the current posture is considered safe and interference-free.

[0053] After verifying that there is no interference between the links, the control system generates a joint spatial interpolation motion trajectory from the current waiting position to the initial observation pose. The trajectory is decomposed into low-level position commands of the control cycle and sent to the servo driver. The servo driver drives the robotic arm to perform spatial displacement, guiding the local 3D camera to the initial observation pose directly above the target workpiece.

[0054] like Figure 3 As shown, in this embodiment, based on the motion state guided by the initial observed pose, step S200 mainly performs multi-source state real-time evaluation and exponential calculation operations to quantify the visual data quality and the robot's physical pose margin, specifically including the following sub-steps: S201, the local 3D camera performs high-frequency dynamic image acquisition under the continuous motion guided by the robotic arm. In this embodiment, the local 3D camera adopts a short-exposure continuous imaging mode, and the exposure time of the local 3D camera is set at the millisecond level to continuously capture the local depth map of the target workpiece at a fixed trigger frequency, such as 30Hz. The short-exposure continuous imaging mode can avoid image blurring caused by the physical movement of the robotic arm. The control system receives the local depth map, and based on the two-dimensional projection boundary of the directed bounding box of the target point cloud cluster generated in step S100 in the current camera coordinate system, it extracts the region of interest containing the target workpiece, and converts the depth pixels in the region of interest into three-dimensional spatial point data to generate a single frame of local point cloud.

[0055] S202, the control system processes a single frame of local point cloud data and calculates the point cloud quality index under the current observation pose. In industrial settings, the surface of target workpieces often exhibits metallic reflective properties, causing specular reflection of structured light or laser beams projected onto the workpiece surface. This results in large areas of voids and surface undulation noise in the point cloud captured by the 3D camera. To quantify the degradation state of visual data, this embodiment constructs a scalar function for the point cloud quality index that integrates the proportion of point cloud spatial integrity and the degree of surface normal vector distortion. Point Cloud Quality Index The calculation formula is as follows: ; In the formula, ; The ideal total number of point clouds within the region of interest is a constant calculated in this embodiment based on the camera resolution and the theoretical area of ​​the workpiece. The ratio represents the number of effective 3D points actually captured in a single frame of local point cloud. The proportion of spatial data integrity in point clouds caused by reflection; To define the total number of neighborhood points within the search radius of a single frame's local point cloud, the search radius is set based on the minimum radius of curvature of the workpiece's surface features to ensure that the neighborhood contains effective feature points that can reflect the surface roughness. For the neighboring region Local normal vectors of valid 3D points; The average normal vector within the local neighborhood; the exponential term calculates the variance of the local normal vector and performs a negative exponential mapping to evaluate the degree of distortion of the surface normal vector; For the natural constant An exponential function with base 0; The L2 norm of a vector is used to represent the vector's L2 norm.

[0056] In this embodiment, the larger the variance, the more severe the burr distortion caused by reflective noise, and the closer the value after negative exponent mapping is to zero; conversely, when the surface is smooth and noise-free, the variance approaches zero, and the value of the exponent term approaches a constant 1. and These are the first normalized weight constant and the second normalized weight constant, respectively, satisfying... The first and second normalized weight constants are assigned values ​​based on the visual inspection task's preference for point cloud data completeness or smoothness. In this embodiment, when the grasping task has high requirements for the workpiece edge contour, the control system assigns a larger value to the first normalized weight constant; when the requirements for workpiece surface curvature calculation are high, the value of the second normalized weight constant is increased.

[0057] S203, during the synchronization period for calculating the point cloud quality index, the control system reads the current joint angle vectors of the robotic arm via the underlying communication bus. In this embodiment, the data reading period is consistent with the underlying servo control period, for example, set to 4ms or 8ms, to ensure the timing alignment of the robot's physical state data and visual state data. The control system reads joint angle vectors containing 6 joint angles. Then, the forward kinematics model is called to analyze the Jacobian matrix under the current configuration. .

[0058] The forward kinematics model and the inverse kinematics model used to solve the target pose are completely different in their calculation direction and function. The inverse kinematics model infers the joint angles from the known Cartesian pose of the end effector; while the forward kinematics model establishes a forward nonlinear spatial mapping function from joint space coordinates to Cartesian space coordinates based on the known dimensional parameters of the robotic arm.

[0059] Jacobian matrix A linear algebraic relationship was established to map joint-space velocities to Cartesian-space velocities. To avoid the robot entering singular configurations or losing motion margins during the optimization process, the control system calculates kinematic maneuverability based on the Jacobian matrix. Kinematic maneuverability The analytical calculation formula is as follows: ; In the formula, The notation for solving the determinant of a matrix; This is the transpose of the Jacobian matrix. The calculated kinematic operability... This is a non-negative scalar. At the physical level, a non-negative scalar represents the volume of the velocity-operating ellipsoid of the end effector in Cartesian space. A larger value indicates greater flexibility for the robotic arm in various spatial directions within its current joint configuration, better physical attitude margin, and a lower likelihood of falling into the singular state of joint lock-up.

[0060] like Figure 4 As shown, in this embodiment, the control system performs an optimization closed-loop operation based on the joint cost function and the sliding response surface in step S300 to solve the problems of point cloud loss and poor robot pose caused by reflection interference. Specifically, it includes the following sub-steps: S301, the control system constructs a joint cost function based on the calculated point cloud quality index and kinematic operability, and performs threshold determination. The point cloud quality index is discrete visual experience data, while kinematic operability is continuous kinematic analytical data; the two have different dimensions. In this embodiment, the control system uses a linear weighted combination method to fuse the two heterogeneous data, and the joint cost function... The calculation formula is as follows: ; In the formula, This represents the current observation pose of the local 3D camera. This is the current joint angle vector of the robotic arm; To observe the point cloud quality index under the pose; To determine the kinematic maneuverability under the corresponding joint configuration; and These are the weight coefficients for the visual term and the weight coefficients for the kinematic term, respectively, satisfying... In actual industrial calibration, the control system sets the weight coefficient of the visual term to be greater than the weight coefficient of the kinematic term, for example, during calibration. It is 0.75 and The value is set to 0.25 to ensure that a clear imaging perspective free from glare is prioritized, while also taking into account the flexibility of the robotic arm's movement.

[0061] After calculating the joint cost function value for the current state, the control system determines whether the joint cost function value meets the set threshold condition. In this embodiment, the threshold condition is that the joint cost function value is greater than a preset lower limit threshold for the cost function. The lower limit threshold for the cost function is set based on the historical statistical average obtained by the system during the initialization phase by performing multiple multi-angle imaging tests on a standard non-reflective workpiece. In physical principle, the statistical average represents the minimum data quality and lower bound of physical pose margin necessary for the system to stably execute the subsequent point cloud registration algorithm.

[0062] When the joint cost function value is greater than the lower limit threshold of the cost function, it indicates that the point cloud obtained by the local 3D camera in the current pose has high integrity and the robotic arm has not approached the singular point. The control system determines that the current state meets the set threshold condition and exits the optimization loop. When the joint cost function value is not greater than the lower limit threshold of the cost function, the control system triggers the sliding window response surface fitting logic and drives the robotic arm to perform micro-motion detection.

[0063] In step S302, when the joint cost function value does not meet the set threshold condition, the control system establishes a sliding window in memory and uses a first-in-first-out (FIFO) cache mechanism to record the historical observation state during the micro-motion detection process. Since the system lacks a priori optimization direction in the initial closed-loop triggering stage, the control system generates an initial micro-motion trajectory based on the current pose. Specifically, starting from the current pose, the control system issues divergent grid detection position commands along the three orthogonal axes of the local Cartesian reference frame and the rotational direction around the three orthogonal axes with a preset step size. The robotic arm runs according to the initial micro-motion trajectory, and the local 3D camera continuously acquires data during the movement. The control system stores the observation pose position vector and the corresponding point cloud quality index acquired in each sampling period as a data node in the sliding window.

[0064] In this embodiment, since the subsequent response surface fitting involves six-dimensional spatial independent variables, the sliding window capacity is set to an integer to ensure that the least squares solution constructs an overdetermined system of equations. The value is set between 30 and 60. When a newly generated data node enters the sliding window and the window is full, the control system discards data nodes with earlier timestamps to keep the total number of data nodes in the window constant. Setting the window capacity range to be greater than the minimum number of equations required for the feature dimension ensures that the data points participating in subsequent fitting contain sufficient spatial distribution information and avoids the failure of local feature smoothing caused by retaining data with earlier timestamps.

[0065] S303, the control system uses historical data cached within a sliding window to fit a quadratic response surface function, thereby estimating the visual gradient vector of the point cloud quality index. The point cloud quality index is an implicit function based on physical imaging principles and cannot be directly differentiated through analytical equations. In the physical lighting model, the specular reflective areas on the workpiece surface exhibit a distribution pattern where reflection is strongest in the central region and nonlinearly decreases towards the outer periphery during spatial observation.

[0066] In this embodiment, the control system employs a quadratic polynomial response surface model to locally approximate the nonlinear physical attenuation law. The control system uses the current observed pose as the origin and extracts the local position deviation vectors of all data nodes within the sliding window relative to the current observed pose. The control system constructs a quadratic polynomial response surface model in the local Cartesian space and fits the mapping relationship between the local position deviation vectors and the point cloud quality index. The quadratic polynomial response surface model is as follows: ; In the formula, It is the local position deviation vector of the observed pose recorded within the sliding window relative to the current observed pose, specifically a six-dimensional column vector containing translation along the three orthogonal axes of space and Euler angle rotation around the three orthogonal axes; The corresponding point cloud quality index is fitted with an estimated value; The coefficients of the constant term; A first-order coefficient vector representing linear changes; This is a second-order symmetric coefficient matrix characterizing nonlinear curvature.

[0067] To find the optimal solution for the coefficients, the control system will move the sliding window within... The data nodes are substituted into a quadratic polynomial response surface model to construct an overdetermined system of equations. The control system then uses a least squares algorithm to perform matrix pseudo-inverse operations, solving for the equation that minimizes the approximation error. , as well as The optimal solution.

[0068] After obtaining the optimal solution for the coefficients, the control system applies the fitted quadratic polynomial response surface model to the current observed pose. Solve for the first-order partial derivatives at the zero vector. Since the quadratic term is... When the vector is zero, the derivative is zero. The gradient direction of the point cloud quality index relative to the spatial position at the current observation pose is determined only by the first-order coefficient vector. The control system will extract the first-order coefficient vector. Visual gradient vector as a point cloud quality index Through response surface methodology, the control system transforms non-differentiable visual physical indices into numerical gradient vectors that can be directly utilized in closed-loop control. This provides the optimal six-dimensional physical space adjustment direction for subsequent trajectory correction, avoiding glare interference and maximizing point cloud quality.

[0069] like Figure 5 As shown, in this embodiment, the control system continues to perform the optimization closed-loop operation based on the joint cost function and the sliding response surface in step S300, synthesizes multi-source heterogeneous gradients and completes the underlying physical control, specifically including the following sub-steps: S304, the control system synchronously calculates the kinematic gradient vector of the kinematic maneuverability and synthesizes a joint gradient vector. Kinematic maneuverability is a continuously differentiable function with respect to joint angle vectors. In this embodiment, the control system directly performs analytical differentiation on the kinematic maneuverability, calculates the partial derivatives of the maneuverability with respect to the six joint angles, and generates a kinematic gradient vector defined in the joint space.

[0070] In a physical sense, the kinematic gradient vector indicates the direction of joint movement that results in the fastest improvement in the overall posture flexibility of the robotic arm when the angles of each joint change slightly. The formula for calculating the analytical partial derivative of the kinematic gradient vector is as follows: ; In the formula, The kinematic gradient vector in joint space; It is the partial derivative operator with respect to the joint angle vector.

[0071] Since the visual gradient vector obtained in step S303 is defined in Cartesian space, the control system needs to map the joint space kinematic gradient vector to Cartesian space to achieve mathematical fusion. Because there is a nonlinear physical mapping between joint space and Cartesian space, direct gradient conversion will result in directional distortion. In this embodiment, the control system uses the principle of static and kinematic duality, calling the transpose of the pseudo-inverse of the Jacobian matrix under the current configuration to achieve lossless conversion of the spatial gradient. The calculation formula for the mapping relationship is as follows: ; In the formula, The kinematic gradient vector mapped to Cartesian space; It is the pseudo-inverse of the Jacobian matrix; This is the transpose operation of the pseudo-inverse matrix. Through algebraic mapping, the control system transforms the joint adjustment intention to improve the robot arm's attitude margin into the motion direction of the end effector in six-dimensional physical space.

[0072] After spatial unification is completed, the control system calls the visual term weight coefficients and kinematic term weight coefficients determined in step S301, and performs linear weighted synthesis of the visual gradient vector and the kinematic gradient vector mapped to Cartesian space to generate a joint gradient vector. The synthesis operation formula is as follows: ; In the formula, This is the synthesized joint gradient vector; The visual gradient vector estimated in step S303; For visual items, the weighting coefficient is... These are the weighting coefficients for the kinematic terms. For the aforementioned weighting coefficients... and In this embodiment, both are set as non-negative calibration constants and typically satisfy normalization conditions. The specific value is determined offline by technicians based on the physical constraints of the actual material loading and unloading scenario: if the workpiece surface has severe reflection or occlusion under the current working conditions, and the dependence on the visual point cloud quality is high, then set... If the current working conditions involve a confined workbench space, the robotic arm is prone to unusual interference, and there is a high degree of dependence on motion flexibility, then set... . The joint gradient vector, in a physical sense, represents the optimal displacement direction that balances avoiding reflective interference with improving the robot's motion flexibility. For the algebraic calculation of the pseudo-inverse of the Jacobian matrix, those skilled in the art can refer to standard linear algebra methods. Matrix decomposition is a well-known technique in this field and will not be elaborated upon here.

[0073] S305, the control system generates a low-level speed control command based on the joint gradient vector, driving the robotic arm to perform adaptive micro-motion trajectory deformation until the closed-loop optimization meets the set threshold condition. In this embodiment, the control system adopts a gradient ascent control strategy, converting the joint gradient vector into the actual physical motion speed. The control system sets a proportional gain coefficient, multiplying the joint gradient vector by the proportional gain coefficient to generate the Cartesian space end-point speed command. The proportional gain coefficient is matched and calibrated based on the response bandwidth of the low-level servo system and the physical control cycle to ensure that the generated control command is within the physical output capability range of the servo motor and to prevent high-frequency resonance.

[0074] To prevent the robotic arm from vibrating due to sudden changes in commands, the control system performs amplitude limiting filtering on the generated Cartesian space end-effector velocity commands. When the calculated velocity modulus exceeds the preset maximum safe linear velocity (e.g., 50 mm / s) or maximum safe angular velocity (e.g., 0.5 rad / s), the control system scales the velocity component proportionally to ensure the smoothness of the physical execution process. The maximum safe linear velocity and maximum safe angular velocity are calibrated based on the end-effector load mass and the equipment's collision protection level to limit instantaneous kinetic energy output.

[0075] After limiting the amplitude, the control system re-substitutes the Cartesian space end-effector velocity command into the inverse mapping relationship of the Jacobian matrix to calculate the underlying joint space velocity command. The physical significance of this operation lies in decomposing the overall motion expectation in space into rotational commands for each independent joint motor. The formula for calculating the joint space velocity command is as follows: ; In the formula, This is the joint space velocity command sent to the underlying servo driver; This is the Cartesian space end velocity command after proportional gain and limiting processing.

[0076] After receiving the joint space velocity command, the servo driver drives the robotic arm to produce continuous physical displacements. Introducing the velocity command corresponding to the joint gradient vector enables the robotic arm to undergo adaptive micro-motion trajectory deformation based on its initial micro-motion trajectory. The robotic arm no longer simply relies on preset meshed movement to find the optimal pose, but instead, guided by the joint gradient vector, autonomously approaches a direction with higher point cloud data quality and without singular pose changes.

[0077] During the micro-motion detection process, the control system repeats steps S301 to S305 at high frequency according to the servo control cycle. Each state update recalculates the cost function and updates the joint gradient vector based on the newly acquired data, correcting the issued velocity control commands. In this embodiment, when the control system determines that the joint cost function value at a certain pose is greater than the lower limit threshold of the cost function, the optimization loop terminates. The control system records the current pose, truncates the underlying velocity control commands, and completes the dynamic adaptive optimization process from initial coarse localization to the final ideal observation pose.

[0078] like Figure 6 As shown, in this embodiment, after completing the micro-motion optimization closed loop in step S300, the control system performs multi-view data fusion and spatial unified mapping operations to extract effective data from the continuous motion trajectory and eliminate coordinate system differences caused by physical space movement. Specifically, this includes the following sub-steps: S401, the control system synchronously extracts multiple frames of local point cloud based on the local extrema of the joint cost function within the continuous micro-motion optimization trajectory. During the micro-motion detection process, the control system records the joint cost function values ​​over time.

[0079] In this embodiment, the control system sets a fixed time comparison window on the time axis. The width of the time comparison window is set based on the running speed of the robotic arm and the sampling frequency of the local 3D camera to cover a complete physical micro-motion cycle. For example, it is set to include a time interval of 5 sampling cycles before and after the current sampling cycle, and the joint cost function value is traversed and recorded.

[0080] When the joint cost function value of a certain sampling period is greater than the function values ​​of all other periods within the time comparison window, the control system determines the pose at the corresponding timestamp as a local extremum of the joint cost function. Physically, a local extremum represents the optimal observation viewpoint with minimal reflection interference and the highest robot posture margin within a continuous physical trajectory. The control system extracts the single-frame local point cloud at the timestamp corresponding to all locally extremum points and stores it in the valid data queue. Setting local extremum determination rules filters out low-quality images in transitional states during the optimization process, avoiding redundant data consuming the processing power of subsequent computing units.

[0081] S402, the control system calls the hand-eye transformation matrix and generates spatial coordinate system transformation matrices for each independent frame point cloud based on the forward kinematics model. The extracted multi-frame local point clouds are each independently defined in the local camera coordinate system at different timestamps, and cannot be directly physically stitched together in space.

[0082] In this embodiment, the control system calls the hand-eye transformation matrix obtained during the system initialization phase. The hand-eye transformation matrix describes the fixed physical relative positional relationship between the local 3D camera coordinate system and the end flange coordinate system of the robotic arm. In physical principle, the hand-eye transformation matrix characterizes the translation and rotation offset of the fixed rigid body relative to the center of the optical center of the local 3D camera and the end flange of the robotic arm.

[0083] The control system reads the joint angle vector corresponding to the timestamp of each frame of point cloud in the valid data queue, inputs the joint angle vector into the forward kinematics model of the robotic arm, and calculates the forward kinematic transformation matrix of the end flange coordinate system of the robotic arm relative to the robot base coordinate system at the moment of acquisition of each frame of point cloud.

[0084] The aforementioned forward kinematics model belongs to the classic mathematical geometry model in the field of robot control. Specifically, in this embodiment, the standard DH parameter method is used to construct the model: by defining local spatial coordinate systems at each link joint of the robotic arm, and based on known physical parameters of the robotic arm, such as link length, link torsion angle, link offset, and the input real-time joint rotation angle, i.e., the joint angle vector, a homogeneous transformation matrix between adjacent links is established; then, by continuously multiplying the homogeneous transformation matrices of all adjacent links, a forward spatial mapping function from the robotic arm base to the end flange can be constructed, and the result of this multiplication derivation is the forward kinematics transformation matrix.

[0085] The control system multiplies the forward kinematic transformation matrix by the hand-eye transformation matrix to generate a spatial coordinate system transformation matrix from the independent frame point cloud to the robot's base coordinate system. The formula for calculating the spatial coordinate system transformation matrix is ​​as follows: ; In the formula, For the first The spatial coordinate system transformation matrix from the local camera coordinate system to the robot base coordinate system corresponding to the local point cloud of a frame; For the first Joint angle vectors recorded at the moment of local point cloud acquisition in a frame; This is the homogeneous transformation matrix of the end flange coordinate system relative to the robot base coordinate system, obtained from the forward kinematics model of the robotic arm; This is a pre-calibrated hand-eye transformation matrix. For the link parameter modeling and algebraic solution of the forward kinematics model of the robotic arm, those skilled in the art can refer to the standard DH parameter method for implementation. The underlying coordinate transformation derivation is a well-known technique in this field and will not be elaborated here.

[0086] In step S403, the control system applies a unified physical mapping rule to unify the coordinate system of multiple frames of local point clouds and performs downsampling and deduplication fusion based on a voxel mesh algorithm. The control system uses the spatial coordinate system transformation matrix calculated in step S402 to perform physical space mapping operations on each frame of point cloud in the effective data queue, unifying the 3D point coordinates to the robot's base coordinate system. The formula for single-point physical space mapping is as follows: ; In the formula, For the first In the local point cloud of the frame, the first A four-dimensional homogeneous coordinate vector representing a 3D point in the local camera coordinate system; This is a four-dimensional homogeneous coordinate vector transformed into the robot's base coordinate system.

[0087] After spatially unified physical mapping, the control system stitches point cloud data from different observation fields into the same three-dimensional space, forming a fused point cloud with a global perspective. Since the observation areas of adjacent views overlap physically, the fused point cloud inevitably contains a large number of redundant overlapping points. The control system uses a voxel grid algorithm to downsample and deduplicate the fused point cloud.

[0088] In this embodiment, the control system constructs a three-dimensional voxel mesh in the robot's base coordinate system, dividing the space into sections with side lengths of... A collection of tiny cubes. Side length Based on the spatial geometric resolution required for subsequent point cloud registration algorithms, for example, set to 1mm to 2mm, the control system calculates the 3D centroid of all overlapping data points contained within each microcube. Before performing the centroid calculation, the control system counts the number of data points inside each microcube, discards microcubes with fewer data points than a preset isolated point threshold, and filters out stray noise points caused by scattering reflections in space.

[0089] The control system replaces all overlapping data points within the corresponding tiny cube with the calculated 3D coordinate centroid. Through voxel mesh downsampling, the control system eliminates physical overlap redundancy caused by multi-viewpoint stitching and fusion while preserving the 3D geometric topological features of the target workpiece surface. It outputs a complete target workpiece point cloud with uniform density distribution and no reflective voids, providing a high signal-to-noise ratio data foundation for the final 3D model matching.

[0090] In this embodiment, after completing the multi-view data fusion and spatial unified mapping in step S400, the control system obtains a complete target workpiece point cloud without reflective holes. Then, it executes the feature registration and job execution operation in step S500 to calculate the final six-degree-of-freedom pose of the target workpiece and issue physical motion commands. Specifically, it includes the following sub-steps: S501, the control system imports the target workpiece template point cloud and performs feature point cloud matching based on the iterative nearest point algorithm. The control system reads the pre-stored standard computer-aided design model of the target workpiece from the local database, discretizes and samples the three-dimensional surface, and generates a template point cloud defined in the workpiece's local coordinate system.

[0091] Considering that the iterative nearest point algorithm is easily affected by the initial spatial pose, in this embodiment, the control system first extracts a fast point feature histogram of the point cloud for coarse registration before performing feature matching. The fast point feature histogram is a spatial topological feature descriptor used to describe the local geometric properties of a 3D point cloud. Its specific definition and construction process are as follows: By calculating the relative spatial distance between each key point in the point cloud and its designated neighborhood data points, as well as the multidimensional spatial angle relationship between their surface normal vectors, these geometric relative relationships are statistically mapped into a multidimensional histogram, thereby forming a local feature description vector with translation and rotation invariance.

[0092] Based on the extracted fast point feature histogram, the specific process of generating the initial spatial transformation matrix through feature matching includes: finding corresponding point pairs with similar local geometric features by comparing the similarity between feature description vectors in the template point cloud and the fused point cloud; eliminating mismatched points caused by noise or symmetrical structures using geometric constraints, and extracting a set of initial feature matching point pairs with high confidence; performing rigid body spatial transformation on the set of high confidence matching point pairs using the random sampling consensus algorithm or singular value decomposition (SVD) algorithm, thereby calculating the initial spatial transformation matrix containing the initial three-dimensional translation vector and the three-dimensional rotation matrix, preventing subsequent iterations from getting trapped in local minima.

[0093] After coarse registration, the control system uses the fused point cloud output in step S403 as the target point set and the template point cloud adjusted by the initial spatial transformation matrix as the source point set to construct a spatial distance objective function. The spatial distance objective function, in physical principle, characterizes the magnitude of the geometric positional deviation between corresponding feature points in space between two sets of point clouds. The formula for calculating the spatial distance objective function of the iterative nearest point algorithm is as follows: ; In the formula, The average Euclidean distance residual between the source point set and the target point set; This represents the total number of valid feature point pairs participating in the matching. The first point cloud in the template A column vector of three-dimensional spatial coordinates of each feature point; This is a column vector of three-dimensional spatial coordinates of the nearest neighbor feature points in the fused point cloud that correspond to the physical counterpart of the template point cloud. Let be the three-dimensional rotation matrix to be solved; Let be the three-dimensional translation vector to be solved.

[0094] In this embodiment, the specific implementation process of the control system executing the iterative nearest point algorithm based on the objective function includes: In the current spatial pose, for each 3D feature point in the source point set, the nearest Euclidean distance corresponding point in the target point set is found using the spatial nearest neighbor search algorithm, constructing the current set of matching point pairs. With minimizing the aforementioned spatial distance objective function as the optimization objective, the optimal 3D rotation matrix and 3D translation vector for the current iteration cycle are solved using the singular value decomposition algorithm. The solved optimal 3D rotation matrix and 3D translation vector are used to perform spatial coordinate transformation on all 3D feature points in the source point set, updating the spatial pose of the source point set. The average Euclidean distance residual for the current iteration cycle is calculated, and it is determined whether the difference between this residual and the residual of the previous iteration cycle is less than a preset convergence threshold, or whether the number of iterations has reached a preset maximum limit. If either condition is met, the algorithm is considered converged, iteration is stopped, and the final multiplied 3D rotation matrix and 3D translation vector are extracted. If the convergence condition is not met, based on the source point set after pose update, the step of finding the nearest corresponding point is returned to start the next iteration until the algorithm converges.

[0095] During the solution process, the control system constructs a K-dimensional tree data structure to accelerate the search for three-dimensional spatial point pairs, uses singular value decomposition to extract rotation and translation transformation parameters, updates the spatial position of the template point cloud, until the change in the average Euclidean distance residual is less than the set convergence tolerance.

[0096] S502, the control system generates the final six-degree-of-freedom pose transformation matrix of the target workpiece in the robot base coordinate system and executes the registration error verification logic. In this embodiment, the control system extracts the three-dimensional rotation matrix and three-dimensional translation vector in the converged state, and assembles the three-dimensional rotation matrix and three-dimensional translation vector into the final six-degree-of-freedom pose transformation matrix of the target workpiece's local coordinate system relative to the robot base coordinate system according to the homogeneous transformation rule.

[0097] To prevent mismatches from causing physical collisions in subsequent operations of the robotic arm, the control system calculates the root mean square error using the final six-DOF pose transformation matrix. Logically, the control system applies the final six-DOF pose transformation matrix to the template point cloud and calculates the square root of the average of the sum of squared Euclidean distances between corresponding feature points in the transformed template point cloud and the fused point cloud.

[0098] The control system compares the calculated root mean square error (RMSE) with a preset physical assembly tolerance threshold, for example, set to 0.5 mm. This physical assembly tolerance threshold is calibrated based on the mechanical clearances and process requirements of upstream and downstream processing equipment in the industrial setting, representing the lower limit of the maximum allowable physical deviation in positioning. When the RMS error is less than the physical assembly tolerance threshold, the control system determines that the registration is valid and locks the final six-degree-of-freedom pose transformation matrix. When the RMS error is not less than the physical assembly tolerance threshold, the control system determines that the precise positioning for loading and unloading has failed, triggers an abnormal alarm command, and suspends subsequent robot physical actions.

[0099] The S503 control system generates a three-dimensional work path based on the final six-degree-of-freedom pose transformation matrix, driving the robotic arm and end effector to complete the physical grasping action. The grasping operation point of the target workpiece has a fixed preset geometric pose in the workpiece's local coordinate system.

[0100] In this embodiment, the control system multiplies the homogeneous matrix corresponding to the grasping operation point by the final six-DOF pose transformation matrix to calculate the absolute target pose of the grasping point in the robot's base coordinate system. The control system offsets outward by a fixed distance, such as 100mm, in the normal direction of the absolute target pose to generate a safe approach point. The value of the fixed distance is set based on the maximum physical extension dimension of the end effector and the geometric thickness of the target workpiece to ensure that the end effector does not interfere with or collide with the surrounding physical environment during the robot arm's approach to the workpiece. The control system plans a continuous, smooth, and interference-free three-dimensional operation path using a polynomial interpolation algorithm, following the order of the current robot arm pose, the safe approach point, and the absolute target pose.

[0101] The polynomial interpolation algorithm in this embodiment uses a fifth-order polynomial interpolation algorithm for spatial trajectory planning in the time domain.

[0102] The specific definition and implementation process is as follows: taking the starting point pose (current robotic arm pose), intermediate transition point pose, and ending point pose as spatial nodes, the path planning is decomposed into a polynomial function of three-dimensional position coordinates and three-dimensional attitude angles with respect to time; the control system sets the velocity vector and acceleration vector of the starting point and ending point to be zero, and sets the velocity and acceleration at the intermediate safe approach point as the boundary condition for continuous and smooth transition; thus, a system of linear equations with unknown coefficients of the polynomial is constructed, and the coefficients of the fifth-order polynomial function are calculated by solving the homogeneous linear equation system, thereby generating a three-dimensional time-pose motion trajectory that changes continuously in the dimensions of position, velocity, and acceleration, ensuring that the robotic arm reaches the absolute target pose accurately without impact or vibration during high-speed movement, and completes the loading and unloading operation.

[0103] During the execution phase, the control system invokes the inverse kinematics model to inversely solve the discrete waypoints in Cartesian space along the three-dimensional work path into joint angle command streams at corresponding moments, which are then sent to the underlying servo drivers of the robotic arm. After the servo drivers guide the end effector to move to the absolute target pose, the control system sends a level trigger signal to the end effector via the digital input / output bus.

[0104] After receiving a level trigger signal, the end effector performs a physical closing action, such as controlling the stepper motor of the electric gripper to rotate forward and close, or turning on the vacuum generator to make the suction cup evacuate air, to complete the loading and unloading of the target workpiece, forming a complete closed loop from vision-guided micro-motion detection and precise positioning to the execution of the robot's loading and unloading physical actions.

[0105] Example 3: One embodiment of the present invention provides an electronic device. This electronic device is intended to serve as the hardware computing platform for the aforementioned control system, for executing the vision-guided robot loading / unloading precision positioning method provided in Embodiment 2.

[0106] The electronic device may include at least one processor, at least one communication interface, at least one memory, and at least one communication bus. The communication bus is used to realize the interconnection and data communication between the above-mentioned hardware components. The communication interface may optionally include a standard industrial Ethernet interface, a serial communication interface, or a wireless communication module, mainly used to establish physical connections with external robotic arm underlying servo drivers, global 3D cameras, and local 3D cameras for status data reading and motion command issuance.

[0107] The memory is primarily used to store the computer program code that runs the aforementioned positioning method, as well as related configuration parameters. The memory may include high-speed random access memory, and may also include non-volatile memory, such as a solid-state drive or at least one disk storage device.

[0108] As the control core of this electronic device, the processor executes all logical operations and device scheduling actions in steps S100 to S500 mentioned in Embodiment 2 by calling the computer program code stored in the memory. Specifically, after reading the instructions, the processor can control the global 3D camera to acquire depth images and analyze coordinates, calculate the point cloud quality index and kinematic maneuverability to construct a joint cost function, drive the robotic arm to perform micro-motion detection by synthesizing a joint gradient vector, and complete feature registration after extracting and fusing multiple frames of point clouds. Finally, it generates a precise work path and drives the robotic arm and end effector to complete the loading and unloading gripping operations.

[0109] Example 4: One embodiment of the present invention provides a computer-readable storage medium. This computer-readable storage medium physically stores computer-executable instructions or a computer program. When the computer-executable instructions or the computer program are read and executed by the processor of the aforementioned electronic device, the vision-guided robot loading and unloading precision positioning method described in detail in Embodiment 2 above can be implemented, achieving the same technical effects and beneficial functions as that method.

[0110] For illustrative purposes, the aforementioned computer-readable storage medium can be non-volatile or volatile. Specifically, the computer-readable storage medium may include read-only memory, programmable read-only memory, random access memory, magnetic disk, optical disk, flash drive, solid-state drive, or any other physical medium capable of stably storing program code. Those skilled in the art will understand that all or part of the steps in the methods of the above embodiments can be implemented by physical hardware related to computer program instructions. The computer program can be stored in the aforementioned various types of computer-readable storage media. When executed, the program not only covers pure software-level algorithm derivation such as visual feature extraction, cost function construction, and spatial transformation matrix solution, but also includes a complete interactive process of sending low-level control signals to external motors and pneumatic actuators based on spatial calculation results to change the actual physical state.

[0111] Specific embodiments have been used to illustrate the principles and implementation methods of this invention. The descriptions of the embodiments above 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 vision-guided robot loading and unloading precision positioning method, characterized in that, include: Acquire global depth images and extract the initial 3D coordinates of the target workpiece to guide the local 3D camera to the initial observation pose; The local 3D camera is used to collect local point cloud quality index data, and the kinematic maneuverability is calculated based on the Jacobian matrix of the robotic arm. A joint cost function is constructed based on the point cloud quality index and the kinematic operability; based on the joint cost function, it is determined whether the current pose meets a set threshold condition. If not satisfied, a joint gradient vector is synthesized based on the point cloud quality index and the kinematic maneuverability, driving the robotic arm to perform micro-motion detection and continuously collect local point clouds until the set threshold condition is met. After the set threshold condition is met, multiple frames of the local point cloud are extracted and mapped to the robot base coordinate system to generate a global fused point cloud; The global fused point cloud is registered with the template point cloud of the target workpiece, and the pose transformation matrix is ​​calculated to drive the robotic arm to complete the loading and unloading operations.

2. The vision-guided robot loading and unloading precision positioning method according to claim 1, characterized in that, The process of synthesizing a joint gradient vector based on the point cloud quality index and the kinematic operability includes: Calculate the visual gradient vector of the point cloud quality index and the kinematic gradient vector of the kinematic operability, respectively. The kinematic gradient vector is mapped from the robot's joint space to Cartesian space; The kinematic gradient vector mapped to Cartesian space is combined with the visual gradient vector to obtain the joint gradient vector used to issue micro-motion commands.

3. The vision-guided robot loading and unloading precision positioning method according to claim 1, characterized in that, The calculation of the point cloud quality index specifically involves constructing a scalar function that integrates the proportion of spatial integrity of the point cloud with the degree of distortion of the surface normal vector, and calculating it according to the following formula: ； In the formula, To be in the current observation pose The point cloud quality index mentioned below, The ideal total number of point clouds within the region of interest is a preset constant. This represents the number of effective 3D points actually captured in a single frame of local point cloud. This is defined as the total number of neighborhood points within the search radius of a local point cloud in a single frame. For the neighboring region Local normal vectors of valid 3D points The average normal vector within the local neighborhood. and These are the first normalized weight constant and the second normalized weight constant, respectively. For the natural constant An exponential function with base 0. The L2 norm of a vector.

4. The vision-guided robot loading and unloading precision positioning method according to claim 1, characterized in that, The construction of the joint cost function, and the determination of whether the current pose meets the set threshold condition based on the joint cost function, specifically includes: The value of the joint cost function is calculated by fusing the point cloud quality index and the kinematic operability using a linear weighted combination method. When the value of the joint cost function is not greater than the preset lower limit threshold of the cost function, it is determined that the preset threshold condition is not met.

5. The vision-guided robot loading and unloading precision positioning method according to claim 4, characterized in that, After determining that the set threshold condition is not met, the method further includes: A sliding window is established, and a first-in-first-out (FIFO) caching mechanism is used to package the current observation pose and the corresponding point cloud quality index acquired in each sampling period into data nodes and store them in the sliding window.

6. The vision-guided robot loading and unloading precision positioning method according to claim 1, characterized in that, The extraction of the local point cloud from multiple frames includes: Set a time comparison window on the time axis and iterate through the values ​​of the joint cost function during the micro-motion detection process; When the value of the joint cost function in any sampling period is the maximum function value within the time comparison window, the pose of the corresponding timestamp is determined to be a local extremum point. Extract the local point cloud corresponding to the local extreme point in a single frame, and store each extracted local point cloud in a valid data queue to form the local point cloud in multiple frames.

7. The vision-guided robot loading and unloading precision positioning method according to claim 1, characterized in that, The process of mapping to the robot's base coordinate system to generate a globally fused point cloud includes: Call the pre-calibrated hand-eye transformation matrix to calculate the spatial coordinate system transformation matrix of the robotic arm end flange coordinate system relative to the robot base coordinate system at the moment of local point cloud acquisition in each frame; The physical space mapping operation is performed on the local point cloud of the multiple frames using the spatial coordinate system transformation matrix, so as to unify the local point cloud of the multiple frames to the robot base coordinate system; A three-dimensional voxel mesh is constructed in the robot's base coordinate system. The three-dimensional coordinate centroid of the overlapping data points within the cube is calculated. The three-dimensional coordinate centroid is used to replace the overlapping data points within the cube. Downsampling and deduplication processing is performed to generate the global fused point cloud.

8. The vision-guided robot loading and unloading precision positioning method according to claim 1, characterized in that, The step of registering the globally fused point cloud with the template point cloud of the target workpiece and solving the pose transformation matrix includes: Extract the fast point feature histograms of the template point cloud and the global fused point cloud for coarse registration to generate an initial spatial transformation matrix; The global fused point cloud is set as the target point set, and the template point cloud adjusted by the initial spatial transformation matrix is ​​set as the source point set. A spatial distance objective function containing a three-dimensional rotation matrix and a three-dimensional translation vector is constructed to execute the iterative nearest point algorithm. The three-dimensional rotation matrix and three-dimensional translation vector in the convergent state are extracted and assembled into the pose transformation matrix.

9. The vision-guided robot loading and unloading precision positioning method according to claim 1, characterized in that, The step of generating the loading and unloading operation path based on the pose transformation matrix includes: The root mean square error between the corresponding feature points of the transformed template point cloud and the globally fused point cloud is calculated using the pose transformation matrix. When the root mean square error is less than the preset physical assembly tolerance threshold, the absolute target pose of the target workpiece gripping point is calculated according to the pose transformation matrix and shifted outward in the normal direction to generate a safe approach point. A three-dimensional operation path is planned and generated using a polynomial interpolation algorithm, and the three-dimensional operation path is determined as the loading and unloading operation path.

10. A vision-guided robot loading and unloading precision positioning system, characterized in that... This includes a robotic arm, a global 3D camera, a local 3D camera, and a control system. The control system is used to execute the vision-guided robot loading and unloading precision positioning method according to any one of claims 1 to 9; The global 3D camera is fixedly mounted above the working area to acquire global depth images; The local 3D camera is mounted on the end effector flange of the robotic arm and is used to acquire local point clouds of the target workpiece.

Images