Dynamic grasping and posture adjusting system of robot dog based on visual servo

Abstract

This invention discloses a visual servoing-based robotic arm system for dynamic grasping and posture adjustment, specifically relating to the field of robot motion control and posture adjustment. It addresses the challenges of balancing target motion uncertainty and overall robot dog stability in complex dynamic environments. The system generates a collaborative workspace for dynamic grasping tasks by predicting the uncertainty envelope of the target's future trajectory. Within this workspace, it discretely samples the robot dog's torso pose to construct a joint cost graph that integrates the robotic arm's operability and foot stability margin. Based on a directed acyclic graph, it performs a heuristic search to plan the torso's posture motion path. Furthermore, it transforms the posture path into a continuous state reference trajectory, combines dynamic stability margin constraints for posture compensation control, and generates posture adjustment commands. This enables stable posture adjustment and dynamic collaborative control of the robot dog during approach and grasping processes.

Description

Technical Field

[0001] This invention relates to the field of robot motion control and posture adjustment technology, and more specifically, to a visual servo-based robotic arm and robot dog dynamic grasping and posture adjustment system. Background Technology

[0002] With the rapid development of intelligent robot technology and computer vision technology, the autonomous operation capability of quadruped robot dogs with integrated robotic arms in complex dynamic environments has gradually become a hot topic in research and industrial applications. In scenarios such as warehousing and logistics, emergency rescue, inspection and maintenance, and hazardous operations, robots often need to perform dynamic grasping operations on moving target objects in irregular terrain, narrow spaces, or environments with frequent human activity.

[0003] However, due to the uncertainty of target motion, environmental perception noise, changes in ground contact conditions, and the inherent dynamic stability constraints of quadrupedal locomotion, the fixed-position grasping or simple trajectory tracking methods commonly used in existing technologies are insufficient to meet the requirements of high-precision and high-reliability dynamic operations. In practical applications, the robot dog needs to continuously adjust its torso posture during continuous walking to ensure that the end effector of the robotic arm remains within reach, while maintaining a reasonable force distribution on the feet to avoid tipping or grasping failure due to posture imbalance. Existing research mainly focuses on single aspects of visual servo control of the robotic arm or gait stabilization control of the quadruped robot, lacking a systematic modeling and unified planning method for the coupling relationship between "torso posture adjustment - robotic arm grasping reachability - overall dynamic stability" in dynamic target scenarios. This makes it difficult to achieve stable, continuous, and high-success-rate collaborative grasping operations under conditions of rapid changes in target motion and high environmental uncertainty.

[0004] Therefore, there is a need for a method that can comprehensively utilize visual prediction information to perform overall torso coordination control based on path selection when the robotic arm of a robot dog grasps dynamic targets, so as to improve the robot dog's autonomous operation capability and adaptability in complex real-world scenarios. Summary of the Invention

[0005] In order to overcome the above-mentioned defects of the prior art, embodiments of the present invention provide a visual servo-based robotic arm robot dog dynamic grasping and posture adjustment system to solve the problems mentioned in the background art.

[0006] To achieve the above objectives, the present invention provides the following technical solution: A vision-servo-based robotic arm and robot dog dynamic grasping and posture adjustment system includes: The target detection module is used to acquire real-time visual measurement data of the target to be captured and to calculate the predicted motion trajectory sequence and trajectory uncertainty envelope of the target to be captured within a set monitoring period. The collaborative space construction module is used to generate a time-varying three-dimensional spatial region based on the predicted motion trajectory sequence and trajectory uncertainty envelope, which serves as the collaborative workspace for the robot dog body and the robotic arm. The cost graph construction module is used to discretely sample the torso pose of the robot dog body within the collaborative workspace. Using the torso pose as an index, the corresponding robotic arm operability index and dynamic stability margin index are bound and stored in a structured form, so that each torso pose sample corresponds to a complete set of performance evaluation parameters, thereby constructing a joint cost graph. The joint cost graph is a structured data set with a one-to-one mapping relationship, using the robot dog torso pose as the index key and the robotic arm operability index and the robot dog dynamic stability margin index as performance evaluation parameters. The path planning module is used to perform heuristic search in a directed acyclic graph based on continuous torso postures. It generates a torso posture motion path from the starting vertex to the target to be grasped by constraining the monotonically increasing coverage of the uncertainty envelope of the trajectory of the robotic arm end effector. The desired posture analysis module is used to extract the corresponding desired torso pose and velocity along the torso posture motion path at a set posture adjustment cycle interval, and generate the tracking state reference trajectory of the robot dog's torso. The attitude compensation module is used to calculate the attitude adjustment amount within the current attitude control cycle with the tracking state reference trajectory as the target, the dynamic stability margin of the corresponding pose in the joint cost map as the constraint, and generate attitude compensation control commands.

[0007] As a further aspect of the present invention, the target detection module calculates the predicted motion trajectory sequence and trajectory uncertainty envelope of the target to be captured within a set monitoring period, specifically including: The system continuously acquires image sequences containing the target to be captured, calculates the real-time pose sequence of the target to be captured relative to the robot dog's base coordinate system, inputs the real-time pose sequence into a trajectory prediction model built based on a recurrent neural network, and outputs the predicted pose of the target to be captured at discrete time points within a set monitoring period, thus forming a predicted motion trajectory sequence. Based on the propagation results of the state estimation covariance matrix within the trajectory prediction model during the set monitoring period, the spatial position uncertainty ellipsoid corresponding to each pose in the predicted motion trajectory sequence is calculated. The uncertainty ellipsoids at all discrete time points are spatially connected and fused in chronological order to form a continuous three-dimensional trajectory uncertainty envelope that envelops the predicted motion trajectory sequence.

[0008] As a further aspect of the present invention, the collaborative space construction module generates a time-varying three-dimensional spatial region as the collaborative workspace between the robot dog body and the robotic arm, specifically including: For each predicted pose of the target to be grasped in the predicted motion trajectory sequence, a momentary task space prism is constructed by extending axially along the approach direction of the robotic arm end effector, with the spatial range of the uncertainty envelope as the basis. The approach direction is determined by the combined constraints of the robotic arm structural parameters of the robotic arm end effector and the normal information of the surface of the target to be grasped; The instantaneous task space prism is formed by axially extending the base contour along the approach direction. The extension length is determined by comprehensively considering the target geometry, the depth of the end effector grasping structure, and the buffer space required for the grasping action, forming a three-dimensional spatial geometry stretched along the approach direction. This three-dimensional spatial geometry is defined as the instantaneous task space prism at the corresponding time point in the predicted motion trajectory sequence. The range of the required position of the robotic arm base when the end effector can reach all points within the instantaneous task space prism under the condition of no singularity is calculated, and this range is expanded outward by a safety margin based on the physical size of the robot dog body and the preset stability boundary to form the instantaneous allowable torso pose region at the corresponding time point in the predicted motion trajectory sequence. The instantaneous allowable torso pose regions corresponding to all discrete time points within the set monitoring period are smoothly connected and fused with voxels in space according to time sequence to obtain a three-dimensional spatial region that changes continuously with future time. The area covered by the line connecting the boundary of this three-dimensional spatial region with the current torso pose of the robot dog is used as the collaborative workspace of the robot dog's torso and robotic arm.

[0009] As a further aspect of the present invention, the cost graph construction module constructs a joint cost graph, specifically including: A three-dimensional voxel mesh is generated in the three-dimensional space corresponding to the collaborative workspace. The center point coordinates and the corresponding horizontal orientation angle of each voxel mesh are virtually set as a candidate robot dog torso pose, forming a torso pose sample set. For each torso pose sample, the volume ratio of the end effector covering the instantaneous task space prism when the robot arm base is located in the torso pose is calculated based on the known robot arm motion parameters, and the ratio value is quantified as a robot arm operability index. The support polygon calculation based on the robot dog calculates the closest distance from the zero moment point to the boundary of the support polygon when the robot dog maintains static balance under the current torso pose, and quantifies it as a dynamic stability margin index. The three-dimensional position, orientation angle, robotic arm operability index, and dynamic stability margin index corresponding to each torso pose sample are associated and stored to construct a joint cost map indexed by torso pose.

[0010] As a further aspect of the present invention, the path planning module generates a torso posture motion path from the starting vertex to the target to be grasped, specifically including: Using the torso pose sample in the joint cost graph as the vertex, if the difference between the two torso pose samples in terms of spatial position and orientation is less than the preset reachable threshold, the spatial distance between the three-dimensional position of the two torso pose samples and the uncertainty envelope of the predicted motion trajectory of the target to be grasped is compared. The torso pose sample that is further away from the target to be grasped is taken as the starting point, and the torso pose sample that is closer to the target to be grasped is taken as the ending point. A directed edge with a clear direction is established between the two to construct a directed acyclic graph. Using the nearest torso pose sample to the robot dog's current real-time torso pose in the joint cost graph as the starting vertex, and the torso pose sample with the optimal cost in the collaborative workspace as the target vertex to be grasped, a heuristic search is performed on the directed acyclic graph. The optimal cost is calculated by weighted fusion based on the stability margin index and the robot arm's operability index. During the search for expanding adjacent vertices, vertices recorded in the joint cost graph and whose coverage of the uncertainty envelope of the trajectory of the target to be grasped increases at the end of the robotic arm are added to the open list. When the search reaches the target vertex, the search backtracks and outputs the ordered vertex sequence from the starting vertex to the target vertex as the torso posture motion path.

[0011] As a further aspect of the present invention, the desired posture analysis module extracts the corresponding desired torso pose and velocity along the torso posture motion path at intervals of a set posture adjustment cycle to generate a torso state reference trajectory, specifically including: The time length of the predicted motion trajectory sequence corresponding to the vertex position of the target to be captured in the torso posture motion path is obtained, the total path tracking time is determined, the time interval of the path point sequence is obtained by dividing the total time by the preset number of posture control cycles, and a timestamp is assigned to each torso pose sample on the torso posture motion path according to this interval. Based on the spatial displacement difference and time interval between torso pose samples at adjacent timestamps, the corresponding linear velocity and angular velocity are calculated as the basis for interpolation. Spline interpolation is used to generate a torso state reference trajectory that is time-continuous and has continuous first derivatives.

[0012] As a further aspect of the present invention, the attitude compensation module calculates the attitude adjustment amount within the current attitude control cycle and generates attitude compensation control commands, specifically including: The total path tracking time is used as the expected control period. The robot dog's current actual body pose and velocity are obtained, and the expected body pose and velocity at each time point within the expected control period are determined by combining the state reference trajectory. Using the pose and velocity deviations between the actual torso and the desired torso as the control target, the dynamic stability margin of the corresponding pose in the joint cost map is transformed into a pose adjustment constraint. The pose adjustment amount within the current pose control cycle is calculated, and the pose adjustment amount is converted into a stability compensation control command.

[0013] The technical effects and advantages of the visual servo-based robotic arm and robot dog dynamic grasping and posture adjustment system of this invention are as follows: This invention achieves overall coordinated control of the robotic arm and the robot dog in dynamic grasping scenarios by unifying and coordinating the modeling and design of visual prediction, collaborative workspace construction, torso posture path planning and foot force optimization control. Under the condition that the target to be grasped is in continuous motion and has significant uncertainty, the robotic arm end effector can effectively cover the target and simultaneously maintain the overall dynamic stability of the robot dog.

[0014] By introducing the target trajectory uncertainty envelope and constructing a time-varying collaborative workspace, the robot dog can perceive the potential future location range of the target, thus providing a reliable constraint basis for torso posture adjustment and motion path planning. Based on a joint cost graph-based posture sampling and path search mechanism, the system ensures foot stability margin while also considering the maneuverability of the robotic arm, achieving collaborative optimization of torso posture evolution and grasping reachability. Furthermore, the system solves for the torso posture adjustment during operation, enabling the robot dog to maintain a smooth and continuous posture adjustment process even under movement and complex contact conditions. This technical solution effectively improves the success rate, continuous operation capability, and motion stability of the robot dog when performing dynamic grasping tasks in complex environments, significantly enhancing the system's comprehensive adaptability to target motion uncertainty and environmental disturbances, and possesses good engineering practical value and promising prospects for widespread application. Attached Figure Description

[0015] Figure 1 This is a schematic diagram of the visual servo-based robotic arm and robot dog dynamic grasping and posture adjustment system of the present invention. Detailed Implementation

[0016] The technical solutions of 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 of ordinary skill in the art without creative effort are within the scope of protection of the present invention.

[0017] Example 1 Figure 1 The present invention provides a visual servoing-based robotic arm and robot dog dynamic grasping and posture adjustment system, comprising: The target detection module is used to acquire real-time visual measurement data of the target to be captured and to calculate the predicted motion trajectory sequence and trajectory uncertainty envelope of the target to be captured within a set monitoring period. The collaborative space construction module is used to generate a time-varying three-dimensional spatial region based on the predicted motion trajectory sequence and trajectory uncertainty envelope, which serves as the collaborative workspace for the robot dog body and the robotic arm. The cost graph construction module is used to discretely sample the torso pose of the robot dog body within the collaborative workspace. Using the torso pose as an index, the corresponding robotic arm operability index and dynamic stability margin index are bound and stored in a structured form, so that each torso pose sample corresponds to a complete set of performance evaluation parameters, thereby constructing a joint cost graph. The joint cost graph is a structured data set with a one-to-one mapping relationship, using the robot dog torso pose as the index key and the robotic arm operability index and the robot dog dynamic stability margin index as performance evaluation parameters. The path planning module is used to perform heuristic search in a directed acyclic graph based on continuous torso postures. It generates a torso posture motion path from the starting vertex to the target to be grasped by constraining the monotonically increasing coverage of the uncertainty envelope of the trajectory of the robotic arm end effector. The desired posture analysis module is used to extract the corresponding desired torso pose and velocity along the torso posture motion path at a set posture adjustment cycle interval, and generate the tracking state reference trajectory of the robot dog's torso. The attitude compensation module is used to calculate the attitude adjustment amount within the current attitude control cycle with the tracking state reference trajectory as the target, the dynamic stability margin of the corresponding pose in the joint cost map as the constraint, and generate attitude compensation control commands.

[0018] In the target detection module, the predicted motion trajectory sequence and trajectory uncertainty envelope of the target to be captured within a set monitoring period are calculated.

[0019] For each frame of image, target detection and 3D localization are performed. Using multi-view visual geometric constraints and time-synchronized calibration parameters, the real-time spatial pose sequence of the target to be captured in the robot dog's base coordinate system is calculated. This pose includes the target's position coordinates and orientation information in 3D space. For the pose time series formed at consecutive time steps, a trajectory prediction model based on a recurrent neural network is constructed. The trajectory prediction model adopts a multi-layer recurrent structure, consisting of an input layer, a feature encoding layer, a temporal recursion layer, and an output regression layer. The input layer receives the target's historical pose sequence over multiple consecutive time steps. The feature encoding layer performs uniform scale normalization and feature embedding mapping on the position and orientation components. The temporal recursion layer uses a gated loop structure to perform temporal modeling of the historical trajectory evolution. The output regression layer generates the predicted target pose at multiple future discrete time points. During the model training phase, a large number of target motion sample trajectories are collected under typical dynamic capture scenarios to construct a supervised training dataset. Real subsequent pose sequences are used as supervision labels, and the model parameters are updated in batches iteratively, enabling the model to learn the trajectory change trends of the target under different motion modes. In actual operation, the latest historical pose sequence acquired in real time is input into the trained recurrent neural network model, which outputs the target prediction pose corresponding to multiple discrete time points within the set monitoring period, thereby forming a complete predicted motion trajectory sequence. This predicted motion trajectory continuously covers the future capture window in the time dimension and reflects the potential motion trend of the target in the spatial dimension.

[0020] After outputting the target predicted trajectory sequence, the state estimation covariance matrix within the recurrent neural network model is extracted synchronously as the basis for uncertainty description. This covariance matrix reflects the statistical distribution characteristics of the model's estimation errors of the target's spatial position and attitude at each prediction time. For each prediction time point within a set monitoring period, based on the covariance matrix corresponding to that prediction time point, the uncertainty components of the target in the three spatial coordinate directions are decomposed, constructing a spatial position uncertainty ellipsoid corresponding to the target's predicted pose. The principal axis direction of the ellipsoid is determined by the eigenvectors of the covariance matrix, and the axis length is determined by the square root of the corresponding eigenvalue, thus forming a geometric body in three-dimensional space that encloses the error range of the prediction point. For all uncertainty ellipsoids generated at discrete prediction time points, spatial connection and continuous fusion processing are performed in chronological order. Transitional voxel regions are constructed between ellipsoids corresponding to adjacent time points. Through interpolation smoothing and morphological fusion operations, local spatial abrupt changes are eliminated, enabling each ellipsoid to form a continuous transition structure on the time axis, ultimately generating a continuous three-dimensional trajectory uncertainty envelope that encloses the predicted trajectory sequence. This uncertainty envelope spatially covers the area where the target may appear during the future monitoring period, and temporally reflects the evolution of the target's motion uncertainty, enabling the robot dog to simultaneously consider the target trajectory trend and prediction error distribution in the subsequent attitude planning and collaborative space construction stages.

[0021] In the collaborative space construction module, a time-varying three-dimensional spatial region is generated as the collaborative workspace of the robot dog body and the robotic arm.

[0022] The predicted poses of the target to be grasped at each discrete time point in the predicted motion trajectory sequence are used as the geometric construction center to establish a corresponding instantaneous task space in the target's local coordinate system. In specific implementation, firstly, based on the uncertainty ellipsoidal space range corresponding to the prediction time point, the cross-sectional profile of the ellipsoid perpendicular to the approach direction of the robotic arm's end effector is extracted, and this cross-sectional profile is used as the geometric base region of the instantaneous task space. The approach direction is determined by the expected approach posture of the robotic arm's end effector during the grasping phase. This expected approach posture is constrained by the robotic arm's structural parameters and the target surface normal information, thereby ensuring that the end effector maintains a stable and continuous spatial approach trajectory when approaching the target. After obtaining the base region, the base profile is axially extended along the approach direction. The extension length is determined comprehensively based on the target's geometric dimensions, the depth of the end effector's grasping structure, and the buffer space required for the grasping action, forming a three-dimensional spatial geometry stretched along the approach direction. This three-dimensional spatial geometry is defined as the instantaneous task space prism corresponding to the prediction time point. By constructing the above method, the instantaneous task space not only covers the entire spatial range where the target may appear at that moment, but also provides a stable spatial buffer for the robotic arm end effector to complete continuous approximation, attitude fine-tuning and grasping actions in this area, thereby ensuring that the subsequent reachability analysis and attitude planning process has sufficient spatial redundancy and safety boundaries.

[0023] After constructing the instantaneous task space prism, for each predicted time point, the required position range of the robotic arm base is further calculated to ensure that the end effector can cover all spatial points within the corresponding instantaneous task space prism under the condition of no singularities. In specific implementation, based on the complete kinematic model of the robotic arm, its workspace accessibility is systematically analyzed. On this basis, the constraint of no singularities is introduced. That is, during the solution of the inverse kinematics of the robotic arm, the continuity and controllability of each joint posture combination are checked, eliminating solution domains with joint limit approximation, Jacobian matrix rank deficiency, or posture abrupt changes. Only the solution set of the robotic arm base space that maintains good control performance and posture continuity throughout the grasping process is retained. Subsequently, this solution set is mapped to the robot dog's torso coordinate system to obtain the position distribution range of the robotic arm base that satisfies both end effector accessibility and no singularities constraints. Further combining the geometric parameters of the robot dog's body, including torso length, width, height, and foot support structure layout, the spatial set of the robotic arm base is extended outward with a certain safety margin to eliminate potential interference caused by ground undulations, changes in foot contact, and micro-vibrations during dynamic movement. Simultaneously, a preset stability boundary constraint is introduced to ensure that the extended spatial range always remains within the robot dog's static and dynamic stable regions. Through the above spatial expansion and stability constraint processing, a corresponding instantaneous permissible torso pose region is ultimately formed at each predicted time point. This torso pose region completely describes the set of all feasible torso poses in three-dimensional space that allow the robot dog to maintain overall stability and satisfy the reachability of the robotic arm's end effector at that moment.

[0024] After obtaining the instantaneous permissible torso pose regions corresponding to all discrete time points within a set monitoring period, spatial smoothing and voxel fusion processing are performed on each time-point region in chronological order to construct a three-dimensional collaborative workspace that continuously changes with future time. Specifically, each instantaneous permissible torso pose region is first uniformly voxelized, mapping it to a discrete spatial set composed of regular three-dimensional voxel units to ensure consistency in spatial representation scale across different time points. Subsequently, temporal connection processing is performed between voxel sets corresponding to adjacent time points. By introducing a spatial interpolation mechanism on the time axis, a continuous transition in voxel distribution between consecutive regions is achieved, ensuring smooth changes in region boundaries during spatial evolution and avoiding abrupt or broken structures. After the continuous fusion of all discrete time-point regions is completed, a three-dimensional collaborative space model that continuously changes in both time and space dimensions is formed. Furthermore, the overall boundary of the spatial region corresponding to the 3D collaborative model and the area covered by the spatial connection between the robot dog's current real-time torso pose are included in the collaborative workspace. This workspace not only includes the range of torso poses allowed during the future target grasping process, but also covers the complete motion channel of the robot dog smoothly transitioning from the current pose to the target grasping pose. Thus, the collaborative activity range of the robot dog and the robotic arm during the entire dynamic grasping process is completely depicted in the spatial structure, providing a unified and continuous spatial constraint basis for subsequent torso pose sampling, path planning, and pose compensation control.

[0025] The cost graph construction module constructs a joint cost graph.

[0026] After constructing the collaborative workspace, a regularized 3D voxel mesh is generated within the corresponding 3D space to discretize the entire spatial region. Based on the geometric dimensions of the collaborative workspace in the 3D direction, the voxel side length resolution is set, and equidistant voxel units are sequentially divided along three orthogonal directions, so that each voxel unit corresponds to a fixed volume region in space. For each voxel unit corresponding to the voxel mesh, its geometric center point coordinates are extracted as candidate translational pose parameters for the robot dog's torso at that spatial position. Simultaneously, considering the adjustable rotation range of the robot dog's torso in the horizontal direction, several discrete horizontal orientation angles are set at the center point. The center point coordinates and the corresponding horizontal orientation angles are combined to virtually define a complete candidate torso pose. Through this method, each voxel unit within the collaborative workspace corresponds to one or more sets of torso pose candidate samples, thus forming a torso pose sample set covering the entire collaborative workspace. This torso pose sample set uniformly covers the allowable area in spatial distribution and fully characterizes the torso orientation changes that the robot dog may take during the grasping process in the posture angle dimension.

[0027] For the constructed set of torso pose samples, for each candidate torso pose sample, based on the structural parameters and kinematic model of the robotic arm, the coverage capability of the robotic arm's end effector to the corresponding instantaneous task space prism under that torso pose is calculated. In the specific implementation, firstly, the torso pose sample is mapped to the robotic arm base coordinate system to determine the spatial position and orientation parameters of the robotic arm base under that torso pose. Then, based on the joint geometry parameters and kinematic constraints of the robotic arm, the reachable workspace of the robotic arm under that robotic arm base pose condition is accurately solved to obtain the three-dimensional spatial range that the end effector can cover. Further, this reachable space is spatially superimposed with the instantaneous task space prism at the corresponding time point, and the proportion of the task space volume actually covered by the end effector to the total volume of the instantaneous task space prism is calculated. This proportion is quantified as the robotic arm operability index corresponding to that torso pose sample. The higher the index value, the stronger the robotic arm's coverage capability of the possible spatial range of the target under that torso pose, thus possessing higher end-effector approach flexibility during dynamic grasping.

[0028] After obtaining the maneuverability index of the robotic arm corresponding to the torso pose sample, a comprehensive stability analysis of the robot dog is further performed on each torso pose sample to calculate the corresponding dynamic stability margin index. Based on the current foot support state of the robot dog, a support polygon formed by the projection positions of each supporting foot on the ground is extracted, and this support polygon is used as the geometric constraint boundary for the robot dog to maintain static balance and dynamic stability. Under this torso pose condition, according to the robot dog's overall mass distribution model, the projection position of its center of mass on the ground plane is calculated, and the shortest geometric distance from the center of mass projection point to the polygon boundary is determined within the support polygon. This shortest distance is used as the dynamic stability margin index corresponding to the current torso pose. This index can intuitively reflect the robot dog's stable bearing capacity against external disturbances and terrain changes in this posture; a larger value indicates higher stability. By performing a unified stability margin calculation on all torso pose samples, a stability evaluation distribution covering the entire collaborative workspace is constructed, so that the subsequent posture planning process can simultaneously meet the overall stability constraints of the robot dog while taking into account the reachability of the robotic arm grasping.

[0029] After obtaining the manipulator's operability index and dynamic stability margin index corresponding to each torso pose sample, the above multi-dimensional information is systematically associated and stored to construct a joint cost graph indexed by torso pose. The three-dimensional spatial coordinates and horizontal orientation angle of the torso pose sample are used as the joint index key, and the corresponding manipulator operability index and dynamic stability margin index are bound and stored in a structured form, ensuring that each torso pose sample corresponds to a complete set of performance evaluation parameters. Furthermore, the joint cost graph is uniformly organized to establish a one-to-one mapping relationship between the spatial index, pose index, and performance index dimensions, thereby supporting rapid querying and evaluation in the subsequent graph search-based posture path planning process. After the joint cost graph is constructed, comprehensive evaluation information on the manipulator's grasping coverage capability and the overall stability of the robot dog can be obtained simultaneously under any torso pose condition. This enables collaborative optimization of the posture planning process under multi-objective constraints, ensuring that the planning results possess both stability and operability at the engineering implementation level.

[0030] The path planning module generates a torso posture motion path from the starting vertex to the target to be grasped.

[0031] After constructing the joint cost graph, all torso pose samples recorded in the joint cost graph are used as vertices in the graph structure to construct a directed acyclic graph describing the continuous evolution of the robot dog's torso pose. In specific implementation, for any two torso pose samples, their 3D spatial position difference and horizontal orientation angle difference are calculated. When both the difference in the spatial translation direction and the difference in the orientation direction are less than a preset reachability threshold, the two torso pose samples are determined to have continuous reachability within a single attitude control cycle. Under the premise of satisfying the continuous reachability condition, the spatial distance between the two torso pose samples and the uncertainty envelope of the predicted trajectory of the target to be grasped is further compared. The torso pose sample spatially farther from the target area is taken as the starting point, and the torso pose sample closer to the target area is taken as the ending point, establishing a directed edge with a clear direction between them. Through this method, the directed edge in the graph structure always points towards the target area in a direction from far to near, thus forming a strict monotonic approximation relationship in the overall graph structure and avoiding loop paths or reverse jumps. The above judgment and edge connection operations are repeatedly performed on all torso pose samples that meet the accessibility conditions in the collaborative workspace. Finally, a directed acyclic graph with directional constraints is constructed. This directed acyclic graph completely describes the topological relationship of all feasible paths for the robot dog's torso pose to continuously evolve from the current state to the target grasping pose region in terms of spatial structure.

[0032] After constructing the directed acyclic graph (DAG), a heuristic search is performed on the DAG structure based on the performance evaluation information of each torso pose sample using the joint cost graph to plan the optimal torso pose motion path. First, the torso pose sample closest to the robot dog's current real-time torso pose is retrieved from the joint cost graph, and the graph vertex corresponding to this sample is set as the search starting vertex. Simultaneously, the torso pose sample with the optimal comprehensive cost is selected within the collaborative workspace, and the graph vertex corresponding to this sample is set as the search target vertex. The comprehensive cost is calculated by weighted fusion of the robotic arm operability index and the dynamic stability margin index. The weighting method is set according to the emphasis on grasping coverage and overall stability in the dynamic grasping task. For example, the weight of robotic arm operability is appropriately increased in high-speed target movement scenarios, and the weight of dynamic stability margin is appropriately increased in complex terrain scenarios, thus enabling the cost evaluation mechanism to flexibly adapt to different task requirements. In the heuristic search process, for each expanded graph vertex, its cumulative path cost and the heuristically estimated cost to reach the target vertex are comprehensively considered. The next search node is selected according to the principle of minimizing total cost, thereby guiding the search process to converge quickly along the cost-optimal direction. Through the above cost fusion and heuristic expansion strategy, the search path can simultaneously ensure the overall stability while continuously improving the target grasping coverage capability of the robotic arm's end effector, thus achieving efficient and stable path planning in complex high-dimensional attitude spaces.

[0033] During the heuristic search, for each adjacent vertex expansion operation, the performance parameters of candidate expansion vertices recorded in the joint cost graph are screened in real time. Only when the coverage of the uncertainty envelope of the target trajectory corresponding to the robotic arm end-effector of the candidate vertex shows an increasing trend relative to the current node is the candidate vertex added to the open list for subsequent search, thus ensuring that the search path always maintains a monotonically increasing reachability to the target during spatial evolution. Simultaneously, visit markers and parent node pointers are established for visited vertices to prevent repeated expansion of the same vertex during the search process, avoiding path backtracking and circular visits. When the search process first reaches the target vertex, according to the optimality principle of the heuristic search algorithm, the path obtained at that moment is the optimal path under the dual conditions of joint cost constraints and monotonic coverage constraints. Subsequently, starting from the target vertex, the path is backtracked level by level along the parent node pointers recorded by each vertex, extracting all the trunk pose samples passed through in sequence, forming an ordered vertex sequence pointing from the starting vertex to the target vertex. Because the search process always maintains a single parent node record rule and terminates the search immediately upon first reaching the target vertex, the final backtracked torso posture motion path is guaranteed to be unique in topology, optimal in terms of cost evaluation, and maintains the monotonic characteristic of continuously approaching the target region in the direction of spatial evolution. This allows the torso posture motion path to serve as the only motion trajectory for the robot dog to perform torso posture adjustment during dynamic grasping.

[0034] In the desired posture analysis module, the desired torso pose and velocity are extracted along the torso posture movement path at intervals of a set posture control cycle, and a torso state reference trajectory is generated.

[0035] After completing the trunk posture motion path planning, the path is uniformly calibrated in the time dimension to form a time reference structure that can be directly used for posture control. First, the predicted motion trajectory sequence time length corresponding to the vertex of the target to be grasped is retrieved from the trunk posture motion path. This time length is determined by the discrete time sequence output by the aforementioned trajectory prediction model within a set monitoring period, serving as the total path tracking time for the entire posture adjustment process. This total path tracking time is used to characterize the complete control cycle experienced by the robot dog as it gradually adjusts from its current trunk state to the final grasping posture. Subsequently, the total path tracking time is equally divided according to a pre-set number of posture control cycles to obtain the time interval between adjacent path points. The number of posture control cycles is set based on a combination of factors, including the dynamic response capability of the robot dog's joint drive system, the overall stable control frequency, and the target's movement speed in the task scenario. For example, in scenarios where the target moves at high speed, the control cycle density is appropriately increased to ensure the continuity and stability of the posture adjustment process. After determining the time intervals, each trunk pose sample on the trunk posture motion path is time-labeled point-by-point according to the time intervals, ensuring that each trunk pose sample corresponds to a unique timestamp. The initial timestamp corresponds to the current real-time torso pose of the robot dog, and the final timestamp corresponds to the final grasping pose. This establishes a strict mapping relationship between the discrete spatial path and the continuous time axis, enabling the torso pose motion path to form a structured and executable sequence of path points in the time dimension, providing a unified time reference for subsequent velocity calculation and trajectory interpolation.

[0036] After completing the path point timestamp allocation, for the torso pose samples corresponding to adjacent timestamps, the motion velocity parameters of the torso within that time interval are calculated segment by segment to construct a complete state reference trajectory. For any two adjacent torso pose samples, the changes in position and orientation in three-dimensional space are extracted, and combined with the time interval between them, the average linear velocity and average angular velocity of the torso within that time interval are calculated. The average linear velocity characterizes the translational evolution of the torso's center of mass in space, and the average angular velocity characterizes the rotational change trend of the torso's orientation over time, thus constructing a discrete velocity sequence covering the entire path tracking cycle. After completing the discrete velocity calculation, the torso pose samples at all timestamps and their corresponding linear and angular velocity information are used as interpolation control points, and spline interpolation is employed to perform continuous processing on the torso pose and velocity sequence. During spline interpolation, a unified constraint is applied to the continuity of the first derivative of adjacent interpolation segments, ensuring the smooth and continuous nature of the generated torso state trajectory in the time dimension. This avoids sudden velocity changes or attitude jumps, thus forming a torso state reference trajectory that is continuous in time and in the sense of its first derivative. This torso state reference trajectory provides the desired torso pose and desired motion velocity at each time step throughout the entire control period, giving the attitude adjustment process a clear temporal evolution law. This provides a stable, continuous, and real-time callable state reference input for the calculation of subsequent attitude compensation control commands.

[0037] The attitude compensation module calculates the attitude adjustment amount within the current attitude control cycle and generates attitude compensation control commands.

[0038] After generating the torso state reference trajectory, the total path tracking time is determined as the expected control period, which serves as the time reference for the entire attitude adjustment process. Specifically, the actual torso pose and velocity information at the current moment are obtained from the robot dog's body state acquisition unit. The torso pose includes the torso's three-dimensional spatial position and orientation in the base coordinate system, and the velocity information includes the corresponding linear and angular velocities. This velocity information is acquired in real-time using a multi-sensor fusion method and aligned with a unified timestamp. Subsequently, the current actual torso pose and velocity are used as the starting state and time-synchronized with the previously generated torso state reference trajectory. The desired torso pose and desired velocity corresponding to each discrete time point within the expected control period are extracted sequentially, forming a desired state sequence covering the entire control cycle. For each attitude control cycle, the actual torso pose and velocity acquired at the start of the attitude control cycle are compared with the desired torso pose and desired velocity extracted from the state reference trajectory at the same time. The pose deviation and velocity deviation within the attitude control cycle are calculated. The pose deviation is used to characterize the degree of deviation between the torso's spatial position and attitude orientation, while the velocity deviation is used to characterize the difference between the torso's motion trend and the desired evolution trajectory.

[0039] After obtaining the pose and velocity deviations within the current attitude control cycle, the dynamic stability margin index recorded in the joint cost map is further introduced to impose stability constraints on the attitude adjustment process. Specifically, based on the desired torso pose corresponding to the current cycle, the dynamic stability margin index corresponding to that desired torso pose is retrieved from the joint cost map. This dynamic stability margin index reflects the safety margin level of the robot dog maintaining overall balance under that torso pose. Subsequently, the dynamic stability margin index is transformed into a constraint boundary for the amplitude of torso pose changes and the rate of attitude adjustment. By limiting the amount of torso spatial displacement change and the amplitude of attitude orientation adjustment within a unit attitude control cycle, the attitude adjustment process is always kept within the allowable range of stability margin, thus constructing attitude adjustment constraints. Based on this, using the aforementioned pose and velocity deviations as control targets, and combined with the stability margin constraints, the torso pose adjustment amount within the current attitude control cycle is calculated. The torso pose adjustment amount includes the torso's position fine-tuning component and attitude angle fine-tuning component in three-dimensional space, used to describe the compensation adjustment amplitude that the torso should perform within the current attitude control cycle. After calculating the attitude adjustment amount, the attitude adjustment amount is input to the attitude control interface module of the robot dog. The attitude adjustment amount is decomposed and mapped according to the whole-body kinematics of the robot dog to generate a corresponding stability compensation control command sequence. The stability compensation control command is output in the form of joint target pose correction amount and attitude adjustment speed command, and is superimposed and fused with the original motion control command of the robot dog. This allows the robot dog to perform real-time compensation and adjustment of the trunk posture deviation while executing the original motion path, thereby maintaining the stability and continuity of the attitude evolution process throughout the entire control period.

[0040] The above embodiments can be implemented, in whole or in part, by software, hardware, firmware, or any other combination thereof. When implemented using software, the above embodiments can be implemented, in whole or in part, as a computer program product. The computer program product includes one or more computer instructions or computer programs. When the computer instructions or computer programs are loaded or executed on a computer, all or part of the processes or functions described in the embodiments of the present invention are generated. The computer can be a general-purpose computer, a special-purpose computer, a computer network, or other programmable device. The computer instructions can be stored in a computer-readable storage medium or transmitted from one computer-readable storage medium to another. For example, the computer instructions can be transmitted from one website, computer, server, or data center to another website, computer, server, or data center via wired (e.g., infrared, wireless, microwave, etc.) means. The computer-readable storage medium can be any available medium that a computer can access or a data storage device such as a server or data center that includes one or more sets of available media. The available medium can be a magnetic medium (e.g., floppy disk, hard disk, magnetic tape), an optical medium (e.g., DVD), or a semiconductor medium. The semiconductor medium can be a solid-state drive.

[0041] Those skilled in the art will recognize that the modules and algorithm steps of the various examples described in conjunction with the embodiments disclosed in this invention can be implemented in electronic hardware, or a combination of computer software and electronic hardware. Whether these functions are implemented in hardware or software depends on the specific application and design constraints of the technical solution. Those skilled in the art can use different methods to implement the described functions for each specific application, but such implementations should not be considered beyond the scope of this invention.

[0042] Those skilled in the art will understand that, for the sake of convenience and brevity, the specific working processes of the systems, devices, and modules described above can be referred to the corresponding processes in the foregoing method embodiments, and will not be repeated here.

[0043] In the several embodiments provided by this invention, it should be understood that the disclosed systems, apparatuses, and methods can be implemented in other ways. For example, the apparatus embodiments described above are merely illustrative; for instance, the division of modules is only a logical functional division, and in actual implementation, there may be other division methods. For example, multiple modules or components may be combined or integrated into another system, or some features may be ignored or not executed. Furthermore, the coupling or direct coupling or communication connection shown or discussed may be through some interfaces; the indirect coupling or communication connection between devices or modules may be electrical, mechanical, or other forms.

[0044] The modules described as separate components may or may not be physically separate. The components shown as modules may or may not be physical modules; they may be located in one place or distributed across multiple network modules. Some or all of the modules can be selected to achieve the purpose of this embodiment according to actual needs.

[0045] In addition, the functional modules in the various embodiments of the present invention can be integrated into one processing module, or each module can exist physically separately, or two or more modules can be integrated into one module.

[0046] If the aforementioned functions are implemented as software functional modules and sold or used as independent products, they can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of this invention, or the part that contributes to the prior art, or a portion of the technical solution, can be embodied in the form of a software product. This computer software product is stored in a storage medium and includes several instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) to execute all or part of the steps of the methods described in the various embodiments of this invention. The aforementioned storage medium includes various media capable of storing program code, such as USB flash drives, portable hard drives, read-only memory (ROM), random access memory (RAM), magnetic disks, or optical disks.

[0047] The above description is merely a specific embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any changes or substitutions that can be easily conceived by those skilled in the art within the scope of the technology disclosed in the present invention should be included within the scope of protection of the present invention.

[0048] In conclusion, the above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the protection scope of the present invention.

Claims

1. A visual servo-based robotic arm robot dog dynamic grasping and posture adjustment system, characterized in that, include: The target detection module is used to acquire real-time visual measurement data of the target to be captured and to calculate the predicted motion trajectory sequence and trajectory uncertainty envelope of the target to be captured within a set monitoring period. The collaborative space construction module is used to generate a time-varying three-dimensional spatial region based on the predicted motion trajectory sequence and trajectory uncertainty envelope, which serves as the collaborative workspace for the robot dog body and the robotic arm. The cost graph construction module is used to discretely sample the torso pose of the robot dog body within the collaborative workspace. Using the torso pose as an index, the corresponding robotic arm operability index and dynamic stability margin index are bound and stored in a structured form, so that each torso pose sample corresponds to a complete set of performance evaluation parameters, thereby constructing a joint cost graph. The joint cost graph is a structured data set with a one-to-one mapping relationship, using the robot dog torso pose as the index key and the robotic arm operability index and the robot dog dynamic stability margin index as performance evaluation parameters. The operability index is obtained by generating a three-dimensional voxel mesh in the three-dimensional space corresponding to the collaborative workspace, and virtually setting the center point coordinates and corresponding horizontal orientation angle of each voxel mesh as a candidate robot dog torso pose to form a torso pose sample set; for each torso pose sample, the volume ratio of the end effector covering the instantaneous task space prism when the robot arm base is located in the torso pose is calculated based on the known robot arm motion parameters, and the volume ratio value is quantified as the robot arm operability index. The path planning module is used to perform heuristic search in a directed acyclic graph based on continuous torso postures. It generates a torso posture motion path from the starting vertex to the target to be grasped by constraining the monotonically increasing coverage of the uncertainty envelope of the trajectory of the robotic arm end effector. The desired posture analysis module is used to extract the corresponding desired torso pose and velocity along the torso posture motion path at a set posture adjustment cycle interval, and generate the tracking state reference trajectory of the robot dog's torso. The attitude compensation module is used to calculate the attitude adjustment amount within the current attitude control cycle with the tracking state reference trajectory as the target, the dynamic stability margin of the corresponding pose in the joint cost map as the constraint, and generate attitude compensation control commands.

2. The visual servo-based robotic arm and robot dog dynamic grasping and posture adjustment system according to claim 1, characterized in that, The target detection module calculates the predicted motion trajectory sequence and trajectory uncertainty envelope of the target to be captured within a set monitoring period, specifically including: The system continuously acquires image sequences containing the target to be captured, calculates the real-time pose sequence of the target to be captured relative to the robot dog's base coordinate system, inputs the real-time pose sequence into a trajectory prediction model built based on a recurrent neural network, and outputs the predicted pose of the target to be captured at discrete time points within a set monitoring period, thus forming a predicted motion trajectory sequence. Based on the propagation results of the state estimation covariance matrix within the trajectory prediction model during the set monitoring period, the spatial position uncertainty ellipsoid corresponding to each pose in the predicted motion trajectory sequence is calculated. The uncertainty ellipsoids at all discrete time points are spatially connected and fused in chronological order to form a continuous three-dimensional trajectory uncertainty envelope that envelops the predicted motion trajectory sequence.

3. The visual servo-based robotic arm and robot dog dynamic grasping and posture adjustment system according to claim 1, characterized in that, The collaborative space construction module generates a time-varying three-dimensional spatial region as the collaborative workspace for the robot dog and the robotic arm, specifically including: For each predicted pose of the target to be grasped in the predicted motion trajectory sequence, a momentary task space prism is constructed by extending axially along the approach direction of the robotic arm end effector, with the spatial range of the uncertainty envelope as the basis. The approach direction is determined by the combined constraints of the robotic arm structural parameters of the robotic arm end effector and the normal information of the surface of the target to be grasped; The instantaneous task space prism is formed by axially extending the base contour along the approach direction. The extension length is determined by comprehensively considering the target geometry, the depth of the end effector grasping structure, and the buffer space required for the grasping action, forming a three-dimensional spatial geometry stretched along the approach direction. This three-dimensional spatial geometry is defined as the instantaneous task space prism at the corresponding time point in the predicted motion trajectory sequence. The range of the required position of the robotic arm base when the end effector can reach all points within the instantaneous task space prism under the condition of no singularity is calculated, and this range is expanded outward by a safety margin based on the physical size of the robot dog body and the preset stability boundary to form the instantaneous allowable torso pose region at the corresponding time point in the predicted motion trajectory sequence. The instantaneous allowable torso pose regions corresponding to all discrete time points within the set monitoring period are smoothly connected and fused with voxels in space according to time sequence to obtain a three-dimensional spatial region that changes continuously with future time. The area covered by the line connecting the boundary of this three-dimensional spatial region with the current torso pose of the robot dog is used as the collaborative workspace of the robot dog's torso and robotic arm.

4. The visual servo-based robotic arm and robot dog dynamic grasping and posture adjustment system according to claim 3, characterized in that, The cost graph construction module constructs a joint cost graph, specifically including: The support polygon calculation based on the robot dog calculates the closest distance from the zero moment point to the boundary of the support polygon when the robot dog maintains static balance under the current torso pose, and quantifies it as a dynamic stability margin index. The three-dimensional position, orientation angle, robotic arm operability index, and dynamic stability margin index corresponding to each torso pose sample are associated and stored to construct a joint cost map indexed by torso pose.

5. The visual servo-based robotic arm and robot dog dynamic grasping and posture adjustment system according to claim 1, characterized in that, The path planning module generates a torso posture motion path from the starting vertex to the target to be grasped, specifically including: Using the torso pose sample in the joint cost graph as the vertex, if the difference between the two torso pose samples in terms of spatial position and orientation is less than the preset reachable threshold, the spatial distance between the three-dimensional position of the two torso pose samples and the uncertainty envelope of the predicted motion trajectory of the target to be grasped is compared. The torso pose sample that is further away from the target to be grasped is taken as the starting point, and the torso pose sample that is closer to the target to be grasped is taken as the ending point. A directed edge with a clear direction is established between the two to construct a directed acyclic graph. Using the nearest torso pose sample to the robot dog's current real-time torso pose in the joint cost graph as the starting vertex, and the torso pose sample with the optimal cost in the collaborative workspace as the target vertex to be grasped, a heuristic search is performed on the directed acyclic graph. The optimal cost is calculated by weighted fusion based on the stability margin index and the robot arm's operability index. During the search for expanding adjacent vertices, vertices recorded in the joint cost graph and whose coverage of the uncertainty envelope of the trajectory of the target to be grasped increases at the end of the robotic arm are added to the open list. When the search reaches the target vertex, the search backtracks and outputs the ordered vertex sequence from the starting vertex to the target vertex as the torso posture motion path.

6. The visual servo-based robotic arm and robot dog dynamic grasping and posture adjustment system according to claim 1, characterized in that, In the desired posture analysis module, along the torso posture motion path, the corresponding desired torso pose and velocity are extracted at intervals of a set posture adjustment cycle to generate a tracking state reference trajectory for the robot dog's torso, specifically including: The time length of the predicted motion trajectory sequence corresponding to the vertex position of the target to be captured in the torso posture motion path is obtained, the total path tracking time is determined, the time interval of the path point sequence is obtained by dividing the total time by the preset number of posture control cycles, and a timestamp is assigned to each torso pose sample on the torso posture motion path according to this interval. Based on the spatial displacement difference and time interval between torso pose samples at adjacent timestamps, the corresponding linear velocity and angular velocity are calculated as the basis for interpolation. Spline interpolation is used to generate a torso state reference trajectory that is time-continuous and has continuous first derivatives.

7. The visual servo-based robotic arm and robot dog dynamic grasping and posture adjustment system according to claim 1, characterized in that, The attitude compensation module calculates the attitude adjustment amount within the current attitude control cycle and generates attitude compensation control commands, specifically including: The total path tracking time is used as the expected control period. The robot dog's current actual body pose and velocity are obtained, and the expected body pose and velocity at each time point within the expected control period are determined by combining the state reference trajectory. Using the pose and velocity deviations between the actual torso and the desired torso as the control target, the dynamic stability margin of the corresponding pose in the joint cost map is transformed into a pose adjustment constraint. The pose adjustment amount within the current pose control cycle is calculated, and the pose adjustment amount is converted into a stability compensation control command.

Images