Dynamic object pose recognition and mechanical arm grabbing control algorithm

Abstract

The invention relates to the technical field of dynamic object pose recognition and mechanical arm grabbing control algorithms, and discloses a dynamic object pose recognition and mechanical arm grabbing control algorithm. The framework comprises a multi-source heterogeneous sensing fusion module, a motion state joint estimation engine, a grabbing feasibility online evaluator and a rolling time domain grabbing controller, and low-delay collaboration is achieved through unified state space modeling and event-driven scheduling. Wherein the sensing module fuses RGB-D and inertial data to output a six-degree-of-freedom pose with covariance, the estimation engine adopts self-adaptive unscented Kalman filtering to recursion a complete motion state, the evaluator combines dynamic constraints to screen feasible grabbing postures, and the controller solves an optimal joint trajectory with obstacle avoidance constraints on line based on a self-adaptive target trajectory. And feedforward compensation is introduced to suppress dynamic disturbance. According to the technical scheme, the real-time performance is guaranteed, and meanwhile the grabbing success rate and robustness of high-speed, non-uniform-speed and sudden turning dynamic objects are remarkably improved.

Description

Technical Field

[0001] This invention belongs to the field of artificial intelligence and robot control technology, specifically relating to the pose recognition of dynamic objects and the control algorithm for robotic arm grasping. Background Technology

[0002] In cutting-edge applications such as modern intelligent manufacturing, intelligent logistics, and human-machine collaboration, the ability of robotic arms to autonomously identify and accurately grasp dynamic target objects has become a key indicator for measuring the level of system intelligence. Compared to traditional grasping tasks targeting static or quasi-static workpieces, the pose recognition and grasping control of dynamic objects faces more complex challenges in the coupling of perception, decision-making, and execution. At its core, the system not only needs to calculate the spatial position and attitude of the moving object with high accuracy in a short time, but also needs to predict its future trajectory based on this, and simultaneously plan a grasping action that meets dynamic constraints, obstacle avoidance requirements, and end effector accessibility, thereby achieving a real-time closed loop across the entire "perception-prediction-planning-control" chain. Performance bottlenecks at any stage of this technology chain can lead to serious consequences such as grasping failure, low efficiency, or even equipment collisions, thus placing extremely high demands on the robustness, timeliness, and coordination of the algorithm.

[0003] Current mainstream technical solutions mostly employ a phased processing paradigm combining vision-based pose estimation algorithms with classic motion planning frameworks. Specifically, firstly, image sequences of the target object are acquired using monocular, binocular, or depth cameras. Through feature extraction, matching, and 3D reconstruction, combined with a known object model or point cloud template, the current six-degree-of-freedom pose is calculated. Subsequently, this pose information is input into a separate trajectory prediction module, typically using a constant velocity model, Kalman filtering, or a simplified recurrent neural network to extrapolate the object's position at several future moments. Finally, based on the prediction results, the robotic arm control system calls path planning algorithms such as RRT*, CHOMP, or time-elastic bands in the configuration or task space to generate a feasible trajectory, and executes the grasping action through the underlying servo controller. This approach is structurally clear, highly modular, and has demonstrated good grasping success rates for low-speed, regularly moving objects in early laboratory environments, thus gaining some application in industry.

[0004] However, with the continuous development of related technologies and the increasingly stringent performance requirements of application scenarios, some inherent characteristics of the above-mentioned technical solutions at the principle level have gradually revealed their limitations in addressing new challenges. Fundamentally, this stems from the strictly sequential architecture of its "perception-prediction-planning" three-stage process, leading to significant information fragmentation and temporal delays between modules. Specifically, the pose recognition module typically outputs discrete pose estimates at a fixed frame rate. These estimates are inevitably affected by factors such as image blurring, occlusion, and lighting changes, introducing noise and lag. The trajectory prediction module extrapolates from these historical observations with errors, and its prediction accuracy rapidly decreases as the prediction time domain lengthens. Furthermore, the motion planning and execution process of the robotic arm itself has non-zero time consumption. When the object moves at high speed or experiences drastic acceleration changes, the actual pose of the object may have significantly deviated from the planning starting point within the time window from perception to execution completion, resulting in a "what you see is not what you get" grasping deviation. Correspondingly, to compensate for this bias, some solutions attempt to introduce a high-frequency replanning mechanism, that is, continuously updating the pose and replanning the trajectory during the grasping process. However, this leads to a surge in computational load, especially under the condition of limited embedded controller resources, which can easily cause planning timeouts or control jitter, thus reducing the overall stability of the system. At a deeper level, existing methods generally treat pose recognition as a purely geometric problem, while treating grasping control as an independent dynamic problem. They fail to establish a joint optimization mechanism between the two at the algorithm level, which means that the improvement in perception accuracy may not be effectively translated into an improvement in grasping success rate, resulting in a suboptimal situation of "high perception, low efficiency".

[0005] Therefore, how to construct a unified algorithm framework that can deeply integrate real-time perception of dynamic object pose, adaptive prediction of motion state, and collaborative planning of robotic arm grasping actions, effectively overcome the problems of information fragmentation and timing mismatch in the traditional serial architecture, and significantly improve the robustness and success rate of grasping objects with high speed, non-uniform speed, or even sudden change of direction while ensuring computational efficiency, has become a key challenge and a technical problem that needs to be solved by those skilled in the art. Summary of the Invention

[0006] This invention provides a pose recognition and robotic arm grasping control algorithm for dynamic objects, aiming to solve the technical problems of information fragmentation, timing mismatch, and low computational efficiency caused by the serial architecture of perception, prediction, and planning in existing technologies. To achieve the above-mentioned objectives, this invention constructs an integrated closed-loop control framework that integrates multimodal perception data, adaptive motion state modeling, and collaborative optimization of grasping actions. Structurally, this framework abandons the traditional one-way data transfer mechanism between modules and instead adopts a joint estimation and rolling optimization strategy based on a unified state space. This significantly improves the robustness and success rate of grasping objects with high speed, non-uniform speed, and even sudden changes in direction while ensuring real-time performance.

[0007] The integrated closed-loop control framework comprises four core functional units: a multi-source heterogeneous sensing fusion module, a motion state joint estimation engine, an online grasping feasibility evaluator, and a rolling time-domain grasping controller. These units achieve low-latency coupling through a shared memory pool and an event-driven scheduling mechanism, and operate synchronously with a fixed control cycle, ensuring that the end-to-end response time from raw sensor data input to end-effector output is strictly limited to a preset threshold.

[0008] The multi-source heterogeneous sensing fusion module is equipped with at least one global shutter depth camera, one six-axis inertial measurement unit (IMU), and one high frame rate visible light camera. These three components achieve time-aligned acquisition through hardware-synchronized trigger signals. The point cloud sequence output by the depth camera is denoised and downsampled, then pixel-level aligned with the visible light image to form an RGB-D data stream. The IMU outputs real-time angular velocity and linear acceleration data of the target object's attached carrier. The RGB-D data stream and the inertial data are input to a feature extraction submodule, which employs a lightweight convolutional neural network structure. Its front end contains three parallel branches that process depth map gradient features, color texture features, and inertial dynamic features, respectively. The outputs of each branch are concatenated and then processed through an attention weight allocation layer to generate a fused feature tensor. This fused feature tensor is fed into a pose regression head, which directly outputs the target object's six-degree-of-freedom pose estimate in the world coordinate system and its covariance matrix. The pose is represented by a combination of a position vector and a unit quaternion, and the covariance matrix is ​​used to quantify the uncertainty of the current estimate.

[0009] The joint motion state estimation engine receives pose estimation sequences and their covariance information from the multi-source heterogeneous sensing fusion module, and combines this with the robot arm's own motion feedback in the base coordinate system to construct an extended state space model. This model incorporates the target object's position, velocity, angular velocity, and external disturbance terms into the state vector, and uses an unscented Kalman filter for recursive updates. The filter's state transition function is obtained by discretizing the rigid body motion differential equations, while the observation equations are directly mapped to the pose estimation output. In each filtering iteration, the system dynamically adjusts the process noise covariance parameter based on the trace value of the current covariance matrix. This reduces the prediction gain to suppress noise amplification when the object's motion is stable, and increases the gain to quickly track state changes when a sudden acceleration change is detected. The resulting optimal state estimate not only includes the current position and attitude but also real-time estimates of velocity, angular velocity, and acceleration, forming a complete kinematic description.

[0010] The online grasping feasibility evaluator, based on the complete state vector output by the joint motion state estimation engine, determines in real time whether a feasible grasping window exists that satisfies physical constraints under the current robotic arm configuration and the target object's motion trend. The evaluator integrates a pre-computed grasping posture database, covering the end effector approach direction, gripping force distribution, and contact stability indices for all potential grasping points on the target object's surface. The evaluation process first substitutes the target object's current pose into the database for matching and retrieval, filtering out a set of candidate grasping postures. Then, using the velocity and acceleration information from the motion state estimation, the reachability of each candidate posture in the future finite time domain is dynamically verified. Verification conditions include joint velocity limits, end effector acceleration constraints, and collision avoidance requirements. Finally, only candidate postures that satisfy all constraints are retained and arranged in descending order of stability indices to form a priority queue. This queue serves as the input reference set for the rolling temporal grasping controller.

[0011] The rolling time-domain grasping controller employs a nonlinear model predictive control architecture, solving a finite-time optimal control problem within each control cycle. Its optimization variable is the sequence of robotic arm joint trajectories, and the objective function consists of three weighted terms: the first is the pose error norm of the end effector at the predicted endpoint relative to the target grasping posture; the second is a joint motion smoothness penalty; and the third is a minimum distance penalty to obstacles. Constraints include joint limits, velocity limits, dynamic equations, and obstacle avoidance geometric constraints. The optimization problem is solved online using a sequential quadratic programming algorithm, executing only the first control command in the solution sequence, with the remaining parts re-optimized in the next cycle. Crucially, the target object trajectory in the prediction model does not assume constant velocity or acceleration, but directly embeds the complete state vector output by the joint motion state estimation engine and its extrapolated trajectory, giving the target motion within the prediction window adaptive time-varying characteristics. Furthermore, before each optimization, the controller calls the latest output of the online grasping feasibility evaluator to dynamically update the target grasping posture, thereby achieving a closed-loop linkage between perception, prediction, and planning.

[0012] Furthermore, a feedforward compensation unit is provided between the rolling time-domain grasping controller and the underlying servo driver. This unit calculates the required additional torque command based on the derivative information of the current joint trajectory and the estimated acceleration of the target object, and superimposes it onto the feedback control law output to counteract the dynamic disturbances caused by the target motion. The gain coefficient of the feedforward compensation is tuned online by the system identification module, which continuously monitors the correlation between the actual joint tracking error and external disturbances, and updates the model parameters using the recursive least squares method.

[0013] In a preferred embodiment of the present invention, the lightweight convolutional neural network structure in the multi-source heterogeneous sensing fusion module is deployed on an edge computing device. Its inference process is accelerated by the TensorRT engine and adopts the INT8 quantization strategy to reduce memory bandwidth requirements. The motion state joint estimation engine and the online grasping feasibility evaluator run in the real-time operating system kernel space to ensure task scheduling determinism. The sequential quadratic programming solver of the rolling time-domain grasping controller adopts a sparse matrix storage format and a warm-start strategy to compress the average solution time to less than 30% of the control cycle.

[0014] The integrated closed-loop control framework eliminates information silos in traditional serial architectures through unified state-space modeling, allowing uncertainties in pose recognition to be directly transmitted to the prediction and planning stages, thereby triggering an adaptive adjustment mechanism. Simultaneously, the combination of rolling optimization strategies and high-frequency state updates effectively overcomes grasping deviations caused by perception lag and execution delays. The entire system achieves stable grasping of complex dynamic scenes such as sudden acceleration changes, non-uniform linear motion, and curved trajectories without relying on high-frequency replanning.

[0015] The technical solution of this invention is manifested at the physical connection level as follows: the depth camera, visible light camera, and inertial measurement unit are connected to the synchronous trigger controller of the main control computer through an industrial-grade GPIO interface; the main control computer communicates with the robotic arm controller via gigabit Ethernet to transmit optimized joint trajectory commands; after receiving the commands, the servo drive module built into the robotic arm controller drives the motors of each joint to perform movements, and feeds back the actual position to the main control computer through an encoder, forming a closed loop. All data exchanges are timestamped and synchronized at the nanosecond level through the PTP precision clock protocol.

[0016] The grasping posture database in the online grasping feasibility evaluator is generated through offline simulation during the system initialization phase. The simulation environment is based on the CAD model and material properties of the target object, employing a force-closed grasping quality evaluation criterion. It iterates through all possible contact configurations, selecting and storing grasping postures with stability indices exceeding a preset threshold. The database supports online incremental updates; when the system detects a new type of object, a local resampling process can be triggered to expand the database coverage within a limited time.

[0017] The dynamic object pose recognition and robotic arm grasping control algorithm described in this invention fundamentally solves the grasping failure problem caused by module fragmentation and timing misalignment in traditional methods by deeply integrating perception, prediction, and planning into a unified optimization framework. While maintaining a controllable computational load, the system achieves high-precision tracking and high-success-rate grasping of dynamic targets, making it suitable for various complex application scenarios such as high-speed sorting on intelligent manufacturing production lines, mobile package grasping in logistics warehousing, and interactive operations in human-machine collaborative environments. Attached Figure Description

[0018] Figure 1 This is a schematic diagram of the integrated closed-loop control framework structure of the dynamic object pose recognition and robotic arm grasping control algorithm described in this invention. Detailed Implementation

[0019] The dynamic object pose recognition and robotic arm grasping control algorithm described in this invention is specifically implemented around an integrated closed-loop control framework. This framework consists of four core functional units: a multi-source heterogeneous sensing fusion module, a motion state joint estimation engine, an online grasping feasibility evaluator, and a rolling time-domain grasping controller. Each unit achieves low-latency coupling through a shared memory pool and an event-driven scheduling mechanism, and operates synchronously with a fixed control cycle, ensuring that the end-to-end response time from raw sensor data input to end-effector output is strictly limited to a preset threshold.

[0020] The multi-source heterogeneous sensing fusion module is equipped with at least one global shutter depth camera, one six-axis inertial measurement unit (IMU), and one high frame rate visible light camera. These three components achieve time-aligned acquisition through hardware synchronization trigger signals. The point cloud sequence output by the depth camera is denoised and downsampled, then pixel-level aligned with the visible light image to form an RGB-D data stream. The IMU outputs the angular velocity and linear acceleration data of the target object's attached carrier in real time. The RGB-D data stream and the inertial data are input to a feature extraction submodule, which employs a lightweight convolutional neural network structure. Its front end contains three parallel branches, processing depth map gradient features, color texture features, and inertial dynamic features, respectively. The outputs of each branch are concatenated and then processed through an attention weight allocation layer to generate a fused feature tensor. This fused feature tensor is fed into a pose regression head, directly outputting the target object's six-degree-of-freedom pose estimate in the world coordinate system and its covariance matrix. The pose is represented by a combination of a position vector and a unit quaternion, and the covariance matrix is ​​used to quantify the uncertainty of the current estimate.

[0021] In a preferred embodiment of the present invention, the lightweight convolutional neural network structure is deployed on an edge computing device. Its inference process is accelerated by the TensorRT engine and employs an INT8 quantization strategy to reduce memory bandwidth requirements. The depth map gradient feature branch uses the Sobel operator to calculate the first derivative of the depth map, extracting object edge contour information. The color texture feature branch uses the MobileNetV2 backbone network to retain shallow high-resolution features to enhance local detail expression. The inertial dynamic feature branch concatenates angular velocity and linear acceleration data from several consecutive frames into a temporal vector, compresses it through a one-dimensional convolutional layer, and maps it to the same dimension as the visual features. The attention weight allocation layer employs a channel attention mechanism. The fusion feature tensor is the sum of the channel-wise multiplication results of each branch feature and its corresponding weight.

[0022] The pose regression head employs a fully connected layer stacked structure, ultimately outputting a seven-dimensional vector. The first three bits represent the position coordinates, and the last four bits are L2 normalized to form a unit quaternion. The covariance matrix is ​​reconstructed from the additional 21 output parameters through Cholesky decomposition, ensuring positive definiteness. This covariance matrix serves as the input for the observation noise covariance in the subsequent joint estimation stage.

[0023] The joint motion state estimation engine receives pose estimation sequences and their covariance information from the multi-source heterogeneous sensing fusion module, and combines this with the robot arm's own motion feedback in the base coordinate system to construct an extended state space model. This model integrates the target object's position, velocity, angular velocity, and external disturbance terms into the state vector.

[0024] The joint motion state estimation engine uses an unscented Kalman filter for recursive updates. In each filtering iteration, the optimal state estimate output includes not only the current position and attitude, but also real-time estimates of velocity, angular velocity, and acceleration, forming a complete kinematic description.

[0025] The online grasping feasibility evaluator, based on the complete state vector output by the joint motion state estimation engine, determines in real time whether a feasible grasping window exists that satisfies physical constraints under the current robotic arm configuration and the target object's motion trend. This evaluator integrates a pre-calculated grasping posture database, covering the end effector approach direction, clamping force distribution, and contact stability indices for all potential grasping points on the target object's surface. The database is generated through offline simulation during system initialization. The simulation environment is based on the target object's CAD model and material properties, employing a force-closed grasping quality evaluation criterion. It iterates through all possible contact configurations, selecting and storing grasping postures with stability indices exceeding a preset threshold. The database supports online incremental updates; when the system detects a new type of object, a local resampling process can be triggered to expand the database coverage within a limited time.

[0026] The evaluation process begins by substituting the target object's current pose into the database for matching and retrieval, filtering out a set of candidate grasping poses. Specifically, by calculating the transformation error between the current pose and the object's local coordinate system corresponding to each grasping pose in the database, entries with errors less than a tolerance threshold are retained. Subsequently, using the velocity and acceleration information from the motion state estimation, the reachability of each candidate pose in the future finite time domain is dynamically verified. Finally, only candidate poses that satisfy all constraints are retained and arranged in descending order of stability indices to form a priority queue. This queue serves as the input reference set for the scrolling temporal grasping controller.

[0027] The rolling time-domain grasping controller employs a nonlinear model predictive control architecture, solving a finite-time optimal control problem within each control cycle. The target object trajectory in the prediction model does not assume constant velocity or acceleration, but rather directly embeds the complete state vector output by the joint motion state estimation engine and its extrapolated trajectory. Specifically, during optimization, the target motion within the prediction window is ensured to have adaptive time-varying characteristics. Furthermore, before each optimization, the controller calls the latest output of the online grasping feasibility evaluator to dynamically update the target grasping posture, thereby achieving a closed-loop linkage between perception, prediction, and planning.

[0028] The optimization problem is solved online using a sequential quadratic programming algorithm. To improve efficiency, a sparse matrix storage format and a warm-start strategy are employed. The initial guess during warm-start is obtained by shifting the solution sequence from the previous cycle forward and padding with zeros. The remaining parts are re-optimized in the next cycle. As a preferred embodiment of the invention, the sequential quadratic programming solver reduces the average solution time to less than 30% of the control cycle.

[0029] Furthermore, a feedforward compensation unit is provided between the rolling time-domain grasping controller and the underlying servo driver. This unit calculates the required additional torque command based on the derivative information of the current joint trajectory and the estimated acceleration of the target object, and superimposes it onto the feedback control law output, which is obtained by coordinate transformation from the acceleration output by the motion state joint estimation engine. The gain coefficient of the feedforward compensation is tuned online by the system identification module.

[0030] At the physical connectivity level, the depth camera, visible light camera, and inertial measurement unit are connected to the synchronous trigger controller of the main control computer via industrial-grade GPIO interfaces. The main control computer communicates with the robotic arm controller via gigabit Ethernet, transmitting optimized joint trajectory commands. After receiving the commands, the servo drive module built into the robotic arm controller drives the motors of each joint to perform movements and feeds back the actual position to the main control computer through encoders, forming a closed loop. All data exchanges are timestamped and synchronized at the nanosecond level using the PTP precision clock protocol.

[0031] To verify the effectiveness of the technical solution of the present invention, the following embodiments and comparative examples were designed.

[0032] In one specific embodiment, the target object is a cube with sides of 10 centimeters, placed on a conveyor belt and moving at variable speeds, with a maximum linear velocity of 1.5 meters per second and a maximum acceleration of 3.0 square meters per second. The robotic arm is a six-degree-of-freedom serial structure with a repeatability accuracy of ±0.05 millimeters. The control system sampling period is set to 20 milliseconds. The multi-source heterogeneous sensing fusion module outputs a pose estimation frequency of 50 Hz. The motion state joint estimation engine operates at 20 Hz. The grasping feasibility online evaluator is synchronized with the rolling time-domain grasping controller within the control cycle.

[0033] The embodiment adopts the integrated closed-loop control framework described in this invention. The comparative example adopts a traditional serial architecture: first, the vision system independently completes pose recognition, with an output frequency of 20 Hz; then, the recognition result is sent to a Kalman filter based on a constant velocity model for trajectory prediction; finally, a static grasping planner generates a one-time grasping trajectory without performing rolling optimization or considering changes in target acceleration.

[0034] In one hundred consecutive grasping experiments, the example successfully grasped 97 times and failed 3 times, all due to unpredictable bouncing of the object at the moment of grasping; the comparative example successfully grasped 68 times and failed 32 times. Among them, 19 times the grasping failed because the predicted trajectory deviated too much from the actual object position, 8 times the grasping failed because the planned trajectory violated the joint speed limit, and 5 times the grasping failed because the object's acceleration was not considered, causing the end to hit the object's surface and slip.

[0035] This invention significantly improves the success rate of data capture while effectively reducing end-to-end system latency and providing rapid response to dynamic changes in the target. Although the single-cycle computational load is slightly higher, rolling optimization avoids high-frequency replanning, resulting in overall computational efficiency superior to traditional methods.

[0036] In a preferred embodiment of the present invention, the joint motion state estimation engine and the online grasping feasibility evaluator run in the kernel space of a real-time operating system, ensuring deterministic task scheduling. The real-time operating system adopts a Xenomai dual-kernel architecture, isolating critical tasks outside the general-purpose Linux kernel, ensuring a worst-case response time of less than five microseconds. The shared memory pool adopts a circular buffer design, with read and write pointers maintained by atomic operations to avoid lock contention. The event-driven scheduling mechanism is based on a priority preemptive strategy, with events that sense data arrival having the highest priority, ensuring timely updates to state estimates.

[0037] Obstacle avoidance constraints in the scrolling temporal capture controller are represented by a Signed Distance Field (SDF) to indicate environmental obstacles. The SDF is built offline during the preprocessing stage, and at runtime, the distance value and gradient of any point are quickly queried through trilinear interpolation.

[0038] In summary, this invention achieves deep integration of perception, prediction, and planning by constructing a joint estimation and rolling optimization strategy in a unified state space. Without relying on high-frequency replanning, the system effectively addresses the challenges of grasping objects with high speed, non-uniform velocity, and sudden changes in direction. It is suitable for various complex application scenarios, such as high-speed sorting on intelligent manufacturing production lines, mobile package grasping in logistics warehousing, and interactive operations in human-machine collaborative environments. Those skilled in the art can adaptively adjust parameters such as hardware platform, network protocol, and optimization weights based on the above embodiments without departing from the core idea of ​​this invention; all such adjustments should be considered within the protection scope of this invention.

Claims

1. A dynamic object pose recognition and robot arm grasping control algorithm, characterized in that, The method is based on an integrated closed-loop control framework to synchronously execute the following steps in fixed control cycles: A multi-source heterogeneous perception fusion step: by time-aligning the collected depth images, visible light images, and inertial measurement data, extracting fusion features, and regressing to obtain the six-degree-of-freedom pose estimation value of the target object in the world coordinate system and its covariance matrix; A motion state joint estimation step: receiving the pose estimation value and its covariance matrix, combining the mechanical arm's own motion feedback, constructing an extended state space model of the state vector containing the target object's position, velocity, angular velocity, and external disturbance term, recursively updating using an unscented Kalman filter, dynamically adjusting the process noise covariance parameter based on the trace value of the current estimated covariance matrix, and outputting a complete motion state vector containing real-time estimation values of position, attitude, velocity, angular velocity, and acceleration; An online evaluation step of grasping feasibility: based on the complete motion state vector, querying a precomputed grasp pose database to filter a candidate grasp pose set, and using the velocity and acceleration information to verify the dynamic feasibility of each candidate pose within a limited time domain in the future, eliminating poses that violate joint velocity limits, end acceleration constraints, or collision avoidance requirements, and sorting the candidate poses that meet all constraints according to stability indicators to form a priority queue; A rolling horizon grasp control step: in each control cycle, dynamically selecting a target grasp pose based on the priority queue, constructing a nonlinear model predictive control problem with the mechanical arm joint trajectory sequence as the optimization variable, the end effector pose tracking error, joint motion smoothness, and obstacle avoidance distance as weighted objectives, and including joint limits, velocity limits, dynamics equations, and obstacle avoidance geometric constraints; wherein the prediction model directly embeds the complete motion state vector and its extrapolated trajectory output by the motion state joint estimation step; the optimization problem is solved online by a sequential quadratic programming algorithm, and only the first segment of the solution sequence is executed.

2. The dynamic object pose identification and robotic arm grasping control algorithm of claim 1, wherein, The multi-source heterogeneous perception fusion step specifically includes: Control at least one global shutter depth camera, one high-frame-rate visible light camera, and one six-axis inertial measurement unit for time-aligned data acquisition through a hardware synchronization trigger signal; Denoise and downsample the point cloud sequence output by the depth camera, and align it with the visible light image at the pixel level to form an RGB-D data stream; Input the RGB-D data stream and the angular velocity and linear acceleration data output by the inertial measurement unit into a lightweight convolutional neural network feature extraction submodule; the front end of the feature extraction submodule includes three parallel branches, which are used to process depth map gradient features, color texture features, and inertial dynamic features, respectively; Concatenate the output features of each branch and generate a fusion feature tensor through a channel attention weight distribution layer; Input the fusion feature tensor into the pose regression head to directly output the six-degree-of-freedom pose estimation value in the form of a position vector and a unit quaternion, and a positive definite covariance matrix reconstructed through additional parameters.

3. The dynamic object pose recognition and robot arm grasp control algorithm of claim 2, wherein: The depth gradient feature branch adopts a Sobel operator to perform first derivative calculation on the depth map to extract edge contour information; The color texture feature branch adopts a MobileNetV2 backbone network to reserve shallow high-resolution features; The inertial dynamic feature branch concatenates angular velocity and linear acceleration data of consecutive frames into a time sequence vector, which is compressed and mapped through a one-dimensional convolution layer; The weight of the channel attention weight distribution layer is calculated by the following formula: after global average pooling of the concatenated features, the weight vector is obtained by sequentially performing linear transformation through a first learnable parameter matrix, ReLU activation function, linear transformation through a second learnable parameter matrix, and Sigmoid function processing; the fusion feature tensor is the sum of the channel-by-channel multiplication results of each branch feature and the corresponding weight.

4. The dynamic object pose identification and robot arm grasping control algorithm of claim 1, wherein, In the motion state joint estimation step, the state vector is defined as a vector containing the target centroid position, linear velocity, angular velocity, and equivalent external acceleration disturbance term; The state transition function is obtained by discretizing the rigid body motion differential equation, and the discrete time step is the fixed control period. The rotation matrix obtained by unit quaternion conversion and the gravity acceleration vector are considered in the state transition equation; The observation equation is directly mapped to the position and unit quaternion in the pose estimate value; The specific strategy for dynamically adjusting the process noise covariance parameter is: presetting a reference process noise covariance, a first adjustment coefficient, a second adjustment coefficient, a first trace value threshold, and a second trace value threshold; when the trace value of the current estimated covariance matrix is less than the first trace value threshold, the process noise covariance parameter is set to the product of the reference process noise covariance and the first adjustment coefficient; When the trace value is greater than the second trace value threshold, the process noise covariance parameter is set to the product of the reference process noise covariance and the second adjustment coefficient; the first adjustment coefficient is less than one, and the second adjustment coefficient is greater than one.

5. The dynamic object pose identification and robotic arm grasping control algorithm of claim 1, wherein, The grasp pose database in the online evaluation step of the grasp feasibility is generated by offline simulation, specifically including: Based on the CAD model and material properties of the target object, a grasp quality evaluation criterion based on force closure is used in the simulation environment to traverse all possible contact configurations and store the grasp poses with stability indicators higher than a preset threshold; the grasp pose information at least includes the approach direction of the end effector, the clamping force distribution, and the contact stability indicator; The database supports online incremental updating. When a new type of object is detected, a local resampling process is triggered to expand the coverage of the database.

6. The dynamic object pose identification and robotic arm grasping control algorithm of claim 1, wherein, The dynamics feasibility verification in the online evaluation step of the grasp feasibility specifically includes: For each candidate grasp pose, according to its fixed transformation relationship relative to the object coordinate system and the pose trajectory of the target object in the future prediction time domain obtained by integrating the complete motion state vector, the time trajectory of the grasp pose in the prediction time domain is calculated; Inverse kinematics is solved for the time trajectory to obtain the corresponding joint space trajectory; The joint space trajectory is verified whether it satisfies the conditions that the joint velocity does not exceed the preset limit, the end acceleration does not exceed the safety threshold, and the robot arm does not collide with the environmental obstacles at any time; the collision detection adopts the combination of the bounding box hierarchy and the GJK algorithm.

7. The dynamic object pose identification and robotic arm grasping control algorithm of claim 1, wherein, In the rolling horizon grasp control step, the objective function of the nonlinear model predictive control problem is specifically: The first term is the norm of the pose error between the end effector at the predicted end point and the target grasp pose, and the pose error is defined as the sum of the Euclidean norm of the position error and the quaternion distance of the attitude error; The second term is the sum of the squares of the joint velocity changes at each step in the prediction horizon, which is used to punish the motion smoothness; The third term is the sum of the reciprocals of the minimum distances between the robot arm and the nearest obstacles at each step in the prediction horizon, wherein a small constant is introduced to prevent division by zero; The first, second and third terms are added after being multiplied by positive weight coefficients respectively to form the total objective function.

8. The dynamic object pose identification and robotic arm grasping control algorithm of claim 1, wherein, The method further comprises a feedforward compensation step: After the joint trajectory instruction is generated in the rolling horizon grasp control step, the feedforward compensation torque is calculated according to the current joint position and velocity, the robot dynamics model parameters, and the acceleration projection of the target object in the end effector coordinate system obtained by coordinate transformation of the complete motion state vector; The feedforward compensation torque is superimposed into the feedback control law output of the underlying servo driver.

9. The dynamic object pose identification and robotic arm grasping control algorithm of claim 8, wherein, The method further comprises a system identification step for online tuning of the dynamics model parameters in the feedforward compensation step: The correlation between the actual joint tracking error and the external disturbance is continuously monitored; The parameter estimates of the inertia matrix, the Coriolis force and the centrifugal force matrix, and the gravity term of the robot are updated online by the recursive least squares method with an update formula with a forgetting factor.

Images