A robot trajectory planning method and device for a pick-and-place task

Abstract

The application discloses a robot trajectory planning method and device for a grasping and placing task. By obtaining a natural language task instruction, path decomposition is performed based on a large language model to generate a stage sequence including pre-grasping, grasping, lifting, pre-placing, placing, and withdrawing, and each stage target and / or constraint. The optimal grasping pose and / or placing pose provided by an upstream module are received, and the stage target related to grasping and / or placing is replaced or corrected. According to the corrected stage target and constraint, a trajectory segment is generated and smoothed to obtain a trajectory sequence. Collision detection is performed on the trajectory sequence. If a collision occurs, collision information is output and local repair or re-planning is performed until the collision detection is passed or a preset re-planning condition is reached. The robot controller executable trajectory sequence is output. The scheme realizes stage decomposition and key pose landing, and forms a collision feedback closed loop to obtain an executable trajectory.

Description

Technical Field

[0001] This application relates to the field of robot trajectory planning technology, and in particular to a robot trajectory planning method and apparatus for grasping and placing tasks. Background Technology

[0002] With the increasing demand for flexible robotic operations in industrial automation and warehousing logistics, robots need to not only determine the target and key poses during grasping and placement tasks, but also generate motion trajectories that can be executed by the robot controller to complete actions such as pre-grasping, grasping, lifting, transporting, pre-placement, placement, and retraction. Robot trajectories typically need to simultaneously satisfy kinematic / dynamic constraints, environmental obstacle avoidance constraints, and task constraints such as end-effector posture maintenance and path traversal area restrictions; otherwise, problems such as unreachability, collisions, or discontinuous motion may occur.

[0003] In existing technologies, motion planning for grasping and placing tasks is usually achieved by “task-level planning + motion-level planning”: the task layer generates action sequences or key points according to the work process, the motion layer generates a trajectory from the starting point to the ending point in Cartesian space or joint space, and achieves speed smoothing through interpolation or time parameterization, and adds collision detection and replanning when necessary. With the development of large language models, some solutions attempt to describe tasks using natural language and generate action steps or planning prompts from the model. However, several shortcomings still exist in engineering implementation: First, the action segments output by the model often remain at the text level or coarse-grained key point descriptions, lacking a stable mapping between them and the executable trajectory of the controller. Second, the "grasping / placement key poses" inferred by the model may be inconsistent with the actual graspable poses output by the upstream perception or grasping modules, leading to end-point alignment deviations or reachability issues during subsequent trajectory execution. Third, if trajectory smoothing and time parameterization are not designed in a unified manner with segment constraints, sudden changes in velocity / acceleration or discontinuous postures can easily occur between segments. Fourth, collision detection and replanning require a closed-loop processing mechanism in complex environments or with dynamic obstacles. However, existing solutions suffer from inconsistent collision information return formats, unclear repair strategies, or unstable replanning trigger conditions, which can easily lead to planning failures or frequent retries, affecting the stability of control execution.

[0004] Among related technologies, Chinese patent publication number CN119974010A discloses a "visual grasping method for robotic arms based on Sim2Real transfer under space-constrained conditions." In this method, the first stage uses Sim2Real to provide the data foundation and initial strategy, while the second stage's sub-decision logic is the core, with a perceptual network providing support for decision-making. This is a step-by-step decision-making, reinforcement learning-driven approach designed to address the differences between the simulated environment (Sim) and the real world (Real), improving the success rate of grasping specific targets in space-constrained, crowded scenarios. Although this patent improves robustness through two-stage training, the grasped target requires a pre-defined color / category, cannot respond to natural language commands, and relies on data generated by the simulated environment; therefore, there is still a gap in real-world scenario transfer.

[0005] Therefore, in robot trajectory planning for grasping and placement tasks, the difficulty in stably converting action segments based on natural language tasks into executable trajectories, the inconsistency between key poses and upstream outputs leading to insufficient trajectory reachability, and the imperfect feedback repair and replanning closed loop after collision detection have become urgent problems to be solved. Summary of the Invention

[0006] This application provides a robot trajectory planning method and apparatus for grasping and placing tasks, aiming to solve the problems in the existing technology for robot trajectory planning for grasping and placing tasks, such as the difficulty in stably converting action segments based on natural language tasks into controller executable trajectories, the inconsistency between key poses and upstream output leading to insufficient trajectory reachability, and the imperfect feedback repair and replanning closed loop after collision detection.

[0007] In a first aspect, a robot trajectory planning method for grasping and placing tasks is provided, the method comprising:

[0008] Obtain task information for the grabbing and placing task, including natural language task instructions;

[0009] The natural language task instructions are decomposed based on a large language model to generate a stage sequence and stage objectives and / or stage constraints corresponding to each stage. The stage sequence includes a pre-fetch stage, a fetch stage, a lift stage, a pre-placement stage, a placement stage, and a pullback stage.

[0010] Receive the optimal grasping pose and / or placement pose provided by the upstream module, and use the optimal grasping pose and / or placement pose to replace or correct the stage targets related to grasping and / or placement in the stage sequence;

[0011] Trajectory segments are generated based on the replaced or corrected stage objectives and / or stage constraints, and the trajectory segments are smoothed to obtain a trajectory sequence that meets the controller input requirements.

[0012] Collision detection is performed on the trajectory sequence; when a collision is detected, collision information is output and local repair or replanning is performed on the trajectory sequence based on the collision information until the collision detection is passed or the preset replanning condition is met;

[0013] The output robot controller can execute a trajectory sequence, the trajectory sequence including time parameters and pose parameters and / or joint angle parameters corresponding to the time parameters, and including velocity parameters and / or acceleration parameters.

[0014] Optionally, in the above scheme, the stage sequence and its stage objectives and / or stage constraints are output in the form of structured data, which includes stage identifier, stage start point, stage end point, stage posture constraints, stage height range constraints, and traversed area restrictions.

[0015] In the above scheme, optionally, the stage sequence is generated by selecting from a preset stage template set, which includes: approach template, alignment template, clamping template, lifting template, transport template, alignment and placement template, release template, and retraction template.

[0016] Optionally, in the above scheme, the replacement or correction includes: replacing the target pose of the grasping stage and the pre-grasping stage with the optimal grasping pose and its offset pose, and replacing the target pose of the placement stage and the pre-placement stage with the placement pose and its offset pose; wherein the offset pose is used to characterize a transition pose that has a preset spatial offset and / or attitude offset from the optimal grasping pose or placement pose.

[0017] In the above scheme, optionally, generating the trajectory segment includes generating the pose trajectory segment of the end effector in Cartesian space and converting the pose trajectory segment into a joint space trajectory segment through inverse kinematics; when there are multiple solutions for inverse kinematics, the joint solution is selected according to the joint limit constraint and / or minimum displacement criterion.

[0018] Optionally, in the above scheme, the smoothing process includes performing one or more of polynomial time parameterization smoothing, spline interpolation smoothing, and / or time scaling processing based on velocity and acceleration boundaries on the trajectory segment to obtain continuous velocity parameters and / or acceleration parameters between segments.

[0019] In the above scheme, optionally, the collision detection is based on the following models: robot body geometric model, end effector geometric model, environmental obstacle geometric model, environmental occupancy grid model, and point cloud occupancy region model; the collision detection includes collision determination of discrete sampled poses in the trajectory sequence and / or collision determination of continuous trajectory segments.

[0020] Optionally, in the above scheme, the collision information includes: the trajectory segment identifier where the collision occurred, the collision sampling point number or time parameter, the collision object identifier, the collision point location, the minimum distance value, and the stage identifier corresponding to the collision.

[0021] Optionally, in the above scheme, the local repair or replanning includes: adjusting the position of the intermediate key point of the trajectory segment where the collision occurred, adjusting the bias pose of the pre-grab or pre-placement, adjusting the height parameter of the trajectory segment, adjusting the end pose constraint, and replacing it with a backup stage template; the preset replanning conditions include at least a maximum replanning number threshold and / or a planning failure alarm condition.

[0022] Secondly, a robot trajectory planning device for grasping and placing tasks, the device comprising:

[0023] The task acquisition module is used to acquire task information for capturing and placing tasks;

[0024] The path decomposition module is used to decompose the natural language task instructions based on the large language model to generate a stage sequence as well as stage objectives and / or stage constraints.

[0025] The pose correction module is used to receive the optimal grasping pose and / or placement pose provided by the upstream module, and to replace or correct the stage targets related to grasping and / or placement.

[0026] The trajectory generation and smoothing module is used to generate trajectory segments and perform smoothing processing based on the replaced or corrected stage objectives and / or stage constraints to obtain a trajectory sequence.

[0027] The collision detection and replanning module is used to perform collision detection on the trajectory sequence, and output collision information and perform local repair or replanning when a collision is detected until the collision detection is passed or the preset replanning conditions are met.

[0028] The trajectory output module is used to output the trajectory sequence that the robot controller can execute.

[0029] Compared with the prior art, this application has at least the following beneficial effects:

[0030] This application, based on further analysis and research of existing technical problems, recognizes that existing technologies in robot trajectory planning for grasping and placement tasks suffer from several issues: difficulty in stably converting natural language task-based action segments into executable trajector trajectories; inconsistencies between key poses and upstream output leading to insufficient trajectory reachability; and imperfect feedback repair and replanning loops after collision detection. This application addresses these issues by utilizing a large language model to decompose natural language task instructions into a sequence of stages including pre-grasping, grasping, lifting, pre-placement, placement, and retraction, providing objectives and constraints for each stage. Thus, the task-level action intent is transformed into a segmented structure directly usable by the planner. This method receives the optimal grasping pose and / or placement pose provided by the upstream module and replaces or corrects the grasping / placement-related stage objectives, grounding the stage objectives from abstract poses inferred from text into concrete trajectories. The upstream output is consistent with the actual key pose, thereby eliminating alignment deviations and accessibility issues caused by inconsistencies in key poses at the segmentation level. Based on this, trajectory segments are generated and smoothed according to the replaced or corrected stage goals and constraints, so that the transition between segments meets the controller's input requirements for trajectory continuity. Subsequently, collision detection is performed on the trajectory sequence, and when a collision is detected, information such as the collision segment and collision point is output to trigger local repair or replanning until the detection is passed or the preset replanning conditions are met. This forms a planning chain of "segmentation, landing, smoothing, and collision closed loop". Therefore, it can address the problems in the background technology, such as the difficulty in stably converting segmented results into executable trajectories, inconsistencies between key poses and upstream outputs, and imperfect feedback repair and replanning mechanisms after collision detection. It outputs a trajectory sequence that meets collision constraints and can be executed by the robot controller. Attached Figure Description

[0031] Figure 1 A flowchart illustrating a robot trajectory planning method for grasping and placing tasks according to an embodiment of this application;

[0032] Figure 2 A schematic diagram showing the connection relationships between the various parts of a robot grasping pose generation system provided in one embodiment of this application;

[0033] Figure 3 A schematic diagram illustrating the workflow of a large language model task parsing module provided in one embodiment of this application;

[0034] Figure 4 A schematic diagram illustrating the workflow of a large target recognition and segmentation module provided in one embodiment of this application;

[0035] Figure 5 A schematic diagram illustrating the workflow of a point cloud computing / reconstruction module provided in one embodiment of this application;

[0036] Figure 6This is a schematic diagram illustrating the workflow of a grasping pose prediction module provided in one embodiment of this application;

[0037] Figure 7 A schematic diagram illustrating the workflow of a motion trajectory planning module provided in one embodiment of this application;

[0038] Figure 8 A flowchart illustrating the target identification and segmentation steps provided in one embodiment of this application;

[0039] Figure 9 A flowchart illustrating the point cloud computing / reconstruction steps provided in one embodiment of this application;

[0040] Figure 10 This is a schematic diagram of the motion trajectory planning steps provided in one embodiment of this application. Detailed Implementation

[0041] To make the objectives, technical solutions, and advantages of this application clearer, the following detailed description is provided in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative and not intended to limit the scope of this application.

[0042] In the description of this application: unless otherwise stated, "a plurality of" means two or more. The terms "first," "second," "third," etc., in this application are intended to distinguish the objects referred to and do not have any special meaning in terms of technical connotation (e.g., they should not be construed as an emphasis on importance or order). Expressions such as "including," "comprising," and "having" also mean "not limited to" (certain units, components, materials, steps, etc.).

[0043] Existing grasping methods still face many challenges in the application of robotic pick-and-place tasks, especially the grasping technology for disordered stacked objects, which is still very immature. The main challenges are:

[0044] Limited applicability to specific tasks: Once traditional robot grasping methods and systems are designed, they can only be used in specific tasks within specific scenarios. They lack applicability to different tasks and scenarios. Even in the same scenario, if the type, shape, or size of the grasped object changes, the algorithm model needs to be redesigned and retrained.

[0045] Limited grasping strategies: Traditional grasping methods often employ fixed grasping strategies, such as fixed grasping points or fixed grasping paths. However, for unordered stacked objects, fixed grasping strategies may not be suitable for all situations. For example, when the position or orientation of the object changes, the fixed grasping point may no longer be applicable, leading to grasping failure.

[0046] Susceptible to occlusion: When an object is occluded by other objects, the robot's vision system has difficulty accurately identifying the position and orientation of the occluded object, making it difficult for the robot to accurately identify the spatial relationship between objects when grasping.

[0047] The lack of fusion and utilization of multimodal information: Traditional methods typically use the RGB-D image information of the object to be grasped as the input to the object recognition and grasping network. They lack fusion strategies for image information with multimodal information such as object segmentation masks, attribute descriptions, spatial location, and grasping strategies, thus limiting the accuracy and flexibility of the grasping method. While the patent with publication number CN 119974010 A improves robustness through two-stage training, the grasping target requires a preset color / category, cannot respond to natural language commands, and relies on data generated in a simulation environment, leaving a gap in real-world scenario transfer. Although the sparse convolutional network in the patent with publication number CN 118254169 A is efficient, it is purely geometrically driven and cannot adapt to multimodal command inputs, resulting in limited generalization ability and scenario applicability. The detection method of the convolutional neural network in the patent with publication number CN 116645636 A is sensitive to background interference, and the use of planar fitting to remove the background may mistakenly delete target object features.

[0048] To address the aforementioned problems, in one embodiment, such as Figure 2 As shown, a robot trajectory planning method for grasping and placing tasks is provided, which obtains task information for grasping and placing tasks, including natural language task instructions;

[0049] The natural language task instructions are decomposed based on a large language model to generate a stage sequence and stage objectives and / or stage constraints corresponding to each stage. The stage sequence includes a pre-fetch stage, a fetch stage, a lift stage, a pre-placement stage, a placement stage, and a pullback stage.

[0050] Receive the optimal grasping pose and / or placement pose provided by the upstream module, and use the optimal grasping pose and / or placement pose to replace or correct the stage targets related to grasping and / or placement in the stage sequence;

[0051] Trajectory segments are generated based on the replaced or corrected stage objectives and / or stage constraints, and the trajectory segments are smoothed to obtain a trajectory sequence that meets the controller input requirements.

[0052] Collision detection is performed on the trajectory sequence; when a collision is detected, collision information is output and local repair or replanning is performed on the trajectory sequence based on the collision information until the collision detection is passed or the preset replanning condition is met;

[0053] The output robot controller can execute a trajectory sequence, the trajectory sequence including time parameters and pose parameters and / or joint angle parameters corresponding to the time parameters, and including velocity parameters and / or acceleration parameters.

[0054] In one implementation, the task information for the grasping and placement task includes at least natural language task instructions, which can be issued by a host computer scheduling system, a human-machine interface terminal, or a production execution system. The task information may also include the robot's current state (joint angles, end effector pose), workstation coordinate system definition, environmental model or obstacle data, and the optimal grasping pose and / or placement pose output by the upstream module. The optimal grasping pose and / or placement pose can be provided by the perception and grasping module, and may take the form of the end effector's position and orientation in a unified coordinate system, or may include gripper openings, grasping type labels, etc., but this embodiment does not limit its source or implementation.

[0055] In one implementation, a path decomposition of natural language task instructions is performed based on a large language model, outputting a stage sequence and corresponding stage objectives and / or stage constraints. The stage sequence may cover at least some or all of the pre-grab, grasp, lift, pre-place, place, and retraction stages. The stage objective for each stage may include target pose or target joint configuration, and the stage constraints may include end-effector pose maintenance, lift height range, traversed area restrictions, speed limits, or safe distance requirements. The path decomposition results are preferably output in a structured format to facilitate direct reading of stage identifiers, start and end conditions, and constraint parameters by the downstream trajectory generation module.

[0056] In one implementation, an optimal grasping pose and / or placement pose provided by an upstream module is received and used to replace or correct the stage targets related to the grasping and / or placement stages. Replacement or correction may include: replacing the target pose of the grasping stage with the optimal grasping pose provided upstream; replacing the target pose of the placement stage with the placement pose provided upstream; and simultaneously correcting the target poses of adjacent pre-grasping / pre-placement stages to ensure spatial continuity of the segmented targets and avoid inconsistencies between key poses inferred solely from text and the actual graspable / placeable poses.

[0057] In one implementation, trajectory segments are generated and smoothed based on the replaced or corrected stage objectives and / or stage constraints. Trajectory segment generation can be performed by generating end-effector pose trajectory segments in Cartesian space or directly generating joint trajectory segments in joint space; when using Cartesian space generation, the joint space trajectory can be obtained through inverse kinematics. Smoothing can employ polynomial time parameterization, spline interpolation, or time scaling under velocity / acceleration boundaries to ensure the trajectory meets controller input requirements and maintains continuity between segments. The output trajectory sequence includes at least time parameters and corresponding pose parameters and / or joint angle parameters, and optionally velocity parameters and / or acceleration parameters.

[0058] In one implementation, collision detection is performed on the trajectory sequence. Collision detection can be performed offline before execution or online during execution; the detection objects include at least the collision relationships between the robot body, the end effector, and environmental obstacles. If a collision is detected, collision information (such as collision segment, collision time / sampling point, collision object, collision point, etc.) is output, and based on this collision information, local repair or replanning is performed on the trajectory sequence, such as adjusting only the trajectory segment where the collision occurred, or replacing the relevant stage template and regenerating the trajectory segment; the planning process ends and the corresponding result is output when the collision detection is passed or a preset replanning condition (such as the maximum number of replanning times) is reached.

[0059] This embodiment decomposes natural language task instructions into a constrained sequence of stages, and replaces or corrects the grasping / placement related stage targets with the optimal grasping pose and / or placement pose provided externally. Combined with trajectory smoothing and closed-loop repair / replanning after collision detection, it can generate an executable trajectory sequence that meets the controller input format and collision constraints.

[0060] In this embodiment, the stage sequence and its stage objectives and / or stage constraints are output in the form of structured data, which includes stage identifier, stage start point, stage end point, stage posture constraints, stage height range constraints, and traversed area restrictions.

[0061] In this embodiment, the stage sequence is generated by selecting from a preset set of stage templates, which includes: approach template, alignment template, clamping template, lifting template, transport template, alignment and placement template, release template, and retraction template.

[0062] In this embodiment, the replacement or correction includes: replacing the target pose of the grasping stage and the pre-grasping stage with the optimal grasping pose and its offset pose, and replacing the target pose of the placement stage and the pre-placement stage with the placement pose and its offset pose; wherein the offset pose is used to characterize a transition pose that has a preset spatial offset and / or attitude offset from the optimal grasping pose or placement pose.

[0063] In this embodiment, generating a trajectory segment includes generating a pose trajectory segment of the end effector in Cartesian space and converting the pose trajectory segment into a joint space trajectory segment through inverse kinematics; when there are multiple solutions for inverse kinematics, a joint solution is selected according to joint limit constraints and / or minimum displacement criteria.

[0064] In this embodiment, the smoothing process includes performing one or more of the following on the trajectory segments: polynomial time parameterization smoothing, spline interpolation smoothing, and / or time scaling based on velocity and acceleration boundaries, to obtain continuous velocity parameters and / or acceleration parameters between segments.

[0065] In this embodiment, the collision detection is based on the following models: robot body geometric model, end effector geometric model, environmental obstacle geometric model, environmental occupancy grid model, and point cloud occupancy region model; the collision detection includes collision determination of discrete sampled poses in the trajectory sequence and / or collision determination of continuous trajectory segments.

[0066] In one implementation, collision detection is based on the robot's body geometry model and the end effector geometry model, combined with the environmental obstacle geometry model. The environmental obstacle geometry model can be derived from tooling CAD, preset geometry (plane, box, cylinder), or an online constructed occupancy grid model / point cloud occupancy region model.

[0067] In one implementation, collision detection can be performed by discretely sampling the trajectory sequence and calculating the collision relationship between the robot link and obstacles for each sampled pose; alternatively, continuous collision detection can be performed on continuous trajectory segments to determine whether the link sweep body intersects with obstacles. The sampling step size can be adaptively set according to trajectory speed, environmental complexity, or minimum safety distance requirements.

[0068] This embodiment performs collision detection on the trajectory sequence before trajectory output or during execution, and supports environment representation based on geometric models or occupancy models. It can perform consistency checks on the obstacle avoidance constraints of the trajectory, providing a basis for subsequent repair or replanning.

[0069] In this embodiment, the collision information includes: the trajectory segment identifier where the collision occurred, the collision sampling point number or time parameter, the collision object identifier, the collision point location, the minimum distance value, and the stage identifier corresponding to the collision.

[0070] In one implementation, when a collision is detected, the planner outputs structured collision information for subsequent use by the repair / replanning module. The collision information may include at least: the identifier of the trajectory segment where the collision occurred (corresponding to which stage or trajectory segment), the collision sampling point number or time parameter (used to locate the location of the collision), the identifier of the collision object (such as the robot link number and obstacle number), the location of the collision point or the contact area, and an optional minimum distance value or safety margin.

[0071] In one implementation, the collision information can also be associated with stage identifiers to clarify that the collision occurred during stages such as pre-grabbing, lifting, or transporting. This allows the repair strategy to adopt different parameter adjustment methods for different stages, such as prioritizing the adjustment of height parameters during the lifting stage and prioritizing the adjustment of detour around intermediate key points during the transport stage.

[0072] This embodiment outputs collision information containing elements such as trajectory segments, time / sampling points, and collision objects, which provides a clear basis for repair or replanning operations and facilitates targeted processing of the local trajectory where a collision occurs.

[0073] In this embodiment, the local repair or replanning includes: adjusting the position of the intermediate key point of the trajectory segment where the collision occurred, adjusting the bias pose of the pre-grab or pre-placement, adjusting the height parameter of the trajectory segment, adjusting the end pose constraint, and replacing it with a backup stage template; the preset replanning conditions include at least a maximum replanning number threshold and / or a planning failure alarm condition.

[0074] In one implementation, local repair involves adjusting only the trajectory segment where the collision occurred while keeping the remaining trajectory segments unchanged. Optional repair strategies include: adjusting the position of the intermediate key point of the trajectory segment (e.g., offsetting it outside the obstacle), adjusting the pre-grab or pre-placement offset pose (changing the approach distance or approach direction), adjusting the height parameters of the trajectory segment (raising it higher or lowering the clearance height), adjusting the end-effector attitude constraints (allowing certain attitude changes to bypass the obstacle), or replacing it with a backup stage template (e.g., replacing straight-line transport with a segmented detour template).

[0075] In one implementation, the replanning condition includes at least a maximum replanning count threshold. When the cumulative number of repairs / replanning attempts reaches the threshold and the collision detection still fails, a planning failure alarm is output or a failure handling process is initiated (e.g., reverting to a safe position, requesting manual intervention, or switching to another task strategy). The maximum number of replanning attempts can be set according to the urgency of the task or real-time requirements.

[0076] This embodiment defines optional local repair / replanning strategies and sets the maximum number of replanning attempts and alarm conditions, which can constrain the trajectory adjustment process after a collision, so that the planning process has a controllable closed-loop processing path in the event of a collision.

[0077] In one embodiment, a closed-loop control method is provided that spans the entire process from natural language command input to automatic robot grasping execution. This method integrates and optimizes multiple modules, including multimodal perception, task parsing, target segmentation, grasping prediction, and motion planning, and deploys AI models for target recognition, segmentation, and grasping pose prediction. It achieves deep fusion and reasoning of multimodal input information such as natural language, RGB images, depth maps, and knowledge bases. This effectively solves the problems of traditional methods being limited by single task requirements and grasping strategies, significantly reducing system reconfiguration costs when tasks change. Furthermore, through cascaded model pipeline design and GPU acceleration, it can adapt to resource-constrained edge deployment.

[0078] This embodiment proposes a modular approach to recognition, prediction, planning, and control throughout the entire process from user command input to robot grasping, based on multimodal inputs such as natural language, RGB-D images, and multi-source knowledge bases. The implementation system includes modules for task parsing, target recognition and segmentation, point cloud computing / reconstruction, grasping pose prediction, robot motion trajectory planning, and robot motion control execution. The connections between these parts are as follows: Figure 2 As shown:

[0079] Module 1: Figure 3 A schematic diagram of the workflow of a Large Language Model (LLM) task parsing module provided in this application embodiment is shown below. Figure 3 As shown, to improve the accuracy of LLM's understanding of user natural language commands and the quality of task parsing, this application uses a large language model architecture based on Retrieval-Augmented Generation (RAG) technology to design a dynamic knowledge base for the robot's working scene, including a domain knowledge base, operation history base, object attribute base, grasping strategy base, scene space description base, etc., which can be dynamically updated. After receiving the user command, the task parsing module first calls the RAG retrieval engine to search the knowledge base, and processes and enhances the effective information retrieved to generate prompt words that the model can better understand and use. The LLM then uses the enhanced prompt words to infer and parse, generating a structured output in JSON format, which includes the name, attributes, spatial orientation, operational constraints, and placement location description of the grasping target object.

[0080] Module 2: Figure 4 This is a schematic diagram illustrating the workflow of a large target recognition and segmentation module provided in an embodiment of this application, as shown below. Figure 4As shown, the text containing the grasped object and attribute enhancement information from the LLM structured output is extracted and simultaneously input into the segmentation model along with the RGB image captured by the camera. An open-set object detection model based on a CNN architecture is employed for end-to-end cross-modal detection, achieving text-to-image alignment and localization. A pixel-level segmentation model is used to infer and output a target object mask. A mask inflation strategy is employed to expand the mask range while preserving background information within a certain range around the target object, thereby improving the detection rate of occluded / blurred targets and better adapting to subsequent pose prediction models.

[0081] Module 3: Figure 5 A schematic diagram of the workflow of a point cloud computing / reconstruction module provided for the application embodiment, such as... Figure 5 As shown, the input data for this step includes a target mask (segmentation mask information from the previous step), a depth map (geometric information), an RGB image (color information), and camera intrinsic parameters (coordinate transformation parameters). First, edge noise in the depth map is eliminated and local holes are filled using filtering and completion strategies. Coordinate transformation is then performed based on the camera intrinsic parameters to convert the depth map into a 3D point cloud. The target point cloud is extracted using target mask filtering (with an additional depth threshold to filter distant points). Finally, point cloud optimization is performed to remove outliers and unify the number of sampling points.

[0082] Module 4: Figure 6 A schematic diagram of the workflow of a grasping pose prediction module provided in the application embodiment is shown below. Figure 6 As shown, a 6DoF pose prediction network model and pre-trained weights for robot gripper grasping are deployed on the end controller. The target object point cloud generated by input module 3 is used to infer multiple candidate grasping poses. Poses that do not meet the requirements are then filtered out based on different task constraints; for example, some tasks are more suitable for grasping in a vertical direction, while others are more suitable for a horizontal direction. Then, collision detection is performed on the remaining poses (through motion planning simulation within the robot's environment model framework). The candidate pose with the highest confidence level is selected as the optimal grasping pose and output as the final prediction result.

[0083] Module 5: Figure 7 A schematic diagram of the workflow of a motion trajectory planning module provided in the application embodiment is shown below. Figure 7 As shown, a complete grasping-placement process of the robot is divided into the following stages: initial pose → pre-grasping pose → grasping pose → control gripper grasping → grasping pose → safe pose → pre-placement pose → placement pose → release gripper → placement pose → safe pose → initial pose.

[0084] Except for the gripper opening and closing process, before controlling the robot's actions in other stages, the motion trajectory planning module first uses Cartesian space trajectory planning (planning position and orientation separately), and then uses fifth-order polynomial interpolation to generate a smooth trajectory. Then, a collision detection algorithm is used for dynamic collision detection, traversing all initial trajectory points and calculating the collision risk. If there is no risk, the trajectory point is given to the motion controller for execution; if there is a risky trajectory segment, the collision point time and position sequence is returned for trajectory repair, until the risk is eliminated before the motion controller is restarted.

[0085] In one embodiment, a closed-loop control method is provided for the entire process from natural language command input to automatic robot grasping and execution, including the following steps:

[0086] Step 1: LLM Task Analysis

[0087] Construct and initialize a multi-source knowledge base for robot working scenarios, including domain knowledge vectors (VectorDB), time-series operation logs (TimeSeriesDB), and object attribute graphs (GraphDB).

[0088] Build a RAG search and enhancement engine.

[0089] Enter the command: "Place a red apple on the white plate."

[0090] Basic Context Extraction: Using the LTP Chinese processing model, the corpus extracts operational entities, actions, quantities, and attribute features from user commands, such as: {'entities': ['a red apple', 'white plate'], 'actions': ['grab', 'put on'], 'quantities': ['one'], 'attributes': ['red', 'white']}

[0091] Query expansion: Add synonyms and related terms to the corpus to broaden the scope of related knowledge retrieval.

[0092] Multi-layer knowledge retrieval: Based on user commands and extended corpus features, the system retrieves multi-source knowledge bases, compresses and extracts relevant domain knowledge, recent successful operation records, and information such as position and size attributes in object maps.

[0093] Security constraint injection: Based on the retrieved valid knowledge entries, obtain the corresponding security constraints from the multi-source knowledge base.

[0094] By comprehensively retrieving and extracting multi-layered knowledge, security constraints, and other content, enhanced large-scale model prompts are generated.

[0095] LLM Inference: Deploy an LLM model, such as Deepseek (or other LLM models), on a local server, and input the above prompts into Deepseek inference to generate structured output instructions.

[0096] Step 2: Target Recognition and Segmentation

[0097] Extract the key-value pair of the target object name ("object_name") from the structured instructions returned by the LLM model in the previous step, such as apple.

[0098] A local edge controller connects to a depth camera and deploys the YOLO-world fast object detection model based on a CNN architecture. The RGB image captured by the camera and the target object are simultaneously input into the YOLO-world model for cross-modal fusion from text to image. The model infers the pixel coordinates of the target object and finally outputs the detection bounding box and its confidence score. Here, the bounding box with the highest confidence score is selected as the valid detection box (x1, y1, x2, y2).

[0099] A lightweight image segmentation model (such as MobileSAM) is deployed on the local controller. Detection box information is used as model input, and a pixel-level mask is output after inference. Since subsequent pose prediction models are more accurate when retaining some background information, the generated mask is dilated. A 20-pixel convolution kernel is constructed, and the generated mask is dilated using this kernel. The dilated mask is then used as the final output, such as... Figure 8 As shown.

[0100] Step 3: Click Cloud Computing / Rebuild

[0101] Data preparation includes target mask, depth map (geometric information), RGB image (color information), and camera intrinsic parameters (coordinate transformation parameters).

[0102] Point cloud conversion: First, the original depth map is preprocessed, and edge noise removal and local hole filling are performed through bilateral filtering and morphological closing operations to obtain the optimized depth. Figure 2 D pixel coordinates (u, v). Obtain the corrected camera intrinsic parameter matrix (fx, fy, cx, cy), and calculate the output 3D point cloud data in (x, y, z) format:

[0103] x = (u - cx) * z / fx;

[0104] y = (v - cy) * z / fy;

[0105] z = depth[u, v];

[0106] Point cloud filtering: The original point cloud is superimposed with the target mask (workspace_mask>0) to filter and extract the point cloud of the target object. At the same time, the invalid background at a distance is filtered by limiting the depth threshold (depth<2m).

[0107] Point cloud optimization: Outliers are removed using lightweight statistical filtering, and a uniform sampling number (3000 points) is achieved through random sampling; if insufficient, sampling is repeated to obtain the final point cloud. The purpose of uniform sampling is to ensure consistency of the output data for the next step of the capture and prediction network. Figure 9 As shown.

[0108] Step 4: Capture Pose Prediction

[0109] Input data: Point cloud data of the target object output in the previous step.

[0110] Grasp pose inference: A GraspNet model is deployed on the local edge controller, pre-trained weights are loaded, and after model inference, multiple candidate grasp poses for the target object are output. Each grasp pose is represented as: gripper center position (x, y, z), rotation matrix (3x3 matrix), gripper width (opening degree), and confidence score.

[0111] Constraint filtering: In order to keep the gripper approaching the target object in an approximately vertical direction, only the gripper normal and the vertical direction with an angle of less than 30° (cosθ<1 / 2) are retained from the multiple candidate poses obtained, and the other poses are discarded.

[0112] Collision detection: Collision detection is performed on the filtered poses (simulated in the robot motion planning framework), and poses with collisions are further discarded.

[0113] Optimal pose output: Select the pose with the highest confidence from the collision-free candidate poses as the optimal grab pose output.

[0114] Step 5: Path decomposition and trajectory planning, such as Figure 10 As shown.

[0115] Throughout the pick-place process, the robot needs to perform multiple complex actions. The motion trajectory planning is divided into two levels: large model path decomposition and motion trajectory planning.

[0116] Large Model Path Decomposition: In step 1, macroscopic segmented paths are generated by the structured output of the deepseek large model inference, and the segmented path ("path_planning") planning results are decomposed from the structured output of the model.

[0117] It is important to note that when performing subsequent motion trajectory planning, in step 1, the [x,y] coordinates of the target object's default position [0, 0, 0] should be replaced with the corresponding values ​​of the gripper center position [x,y,z] of the best predicted pose output by the gripper pose prediction module; in step 2, the target position should be replaced with the gripper center position [x,y,z] of the best predicted pose output by the gripper pose prediction module, and the target pose should be the pose rotation matrix of the best predicted pose.

[0118] Motion trajectory planning: For each decomposed path segment, a fixed time constraint is applied. Motion trajectory planning for position and attitude changes is performed in Cartesian space. A smooth trajectory of the velocity curve is generated using a fifth-order polynomial calculation, and collision detection is performed on the trajectory. This includes the following stages:

[0119] Parameter initialization: Set the total trajectory time parameter (T), and obtain the robot's current Cartesian pose (q1) and target Cartesian pose (q2).

[0120] Trajectory model parameter configuration: Position path: Cartesian space straight line path planning; Attitude interpolation: Target-current attitude quaternion rotation interpolation; Velocity smoothing: Velocity curve calculated based on fifth-order polynomial (ensuring position, attitude values ​​and velocity continuity); Combined trajectory: Fusing position, attitude and velocity parameters to form a complete trajectory mathematical model.

[0121] Create a trajectory planner: Load the configured trajectory mathematical model (position + attitude + fifth-order polynomial velocity), and initialize the planning time step parameter: ΔT=0.008.

[0122] Trajectory sequence generation: Calculate the sampled time series (ΔT / 0.008 + 1), traverse each time series t_i (i=1 to N), call the trajectory planner, and calculate the trajectory array at time t_i (including the position, attitude, and velocity of each time point).

[0123] Collision Detection: Load the robot's environment model (env) and perform collision detection for each trajectory sequence. If a collision is detected, return to replan the trajectory and repair the points. If the replanning fails after 5 attempts, an alarm is triggered.

[0124] Collision detection passes, generating an executable trajectory sequence (including time, position, attitude, and velocity).

[0125] Step 6: Motion control execution

[0126] The motion trajectory sequence generated in the previous step is sent to the robot control module of the end controller to control the robot to execute the action of each segment of the path and complete the entire Pick-and-Place motion process.

[0127] This embodiment proposes a closed-loop control method for the entire process from natural language command input to automatic robot grasping and execution. It integrates and optimizes multiple modules such as multimodal perception, task parsing, target segmentation, grasping prediction, and motion planning. It can effectively solve the problems of "information silos", limited task adaptability, and single grasping strategy of traditional methods, and provide a flexible intelligent solution for robot pick-and-place tasks.

[0128] This embodiment adopts a layered system architecture that combines an LLM-RAG task parsing module based on a large language model with a real-time inference model. On the one hand, semantic understanding is enhanced through a dynamic knowledge base, parsing natural language instructions into structured task parameters and improving scenario applicability; on the other hand, optimized designs such as cascaded pipelines, open vocabulary detection, mask inflation, and collision detection in the real-time inference model improve the target object detection rate and grasping success rate in disordered scenarios.

[0129] This embodiment provides a robot pick-and-place task implementation method that integrates multimodal input information such as natural language, multi-source knowledge base, RGB image, depth map, and target point cloud. The implementation system of this method includes modules such as task parsing, target recognition and segmentation, point cloud computing / reconstruction, grasping pose prediction, robot motion trajectory planning, and robot motion control execution. This method adopts a layered system architecture that combines an LLM-RAG task parsing layer based on a large language model with a real-time inference layer. The task parsing layer LLM model and multi-source database are deployed on the server, combining user input with the multi-source knowledge base to enhance semantic understanding, and parsing natural language instructions into structured task parameters, which are then passed to the lower-level real-time inference model for use. The real-time inference layer model is deployed on the edge controller and includes a machine vision detection model, an object segmentation model, a gripper pose prediction model, and a motion trajectory planning model. The vision detection model outputs a localization bounding box to the object segmentation model. The segmentation mask output by the object segmentation model is inflated to a certain extent before point cloud computing. During point cloud computing, edge noise removal and local hole filling are performed through bilateral filtering and morphological closing operations. After mask filtering, outliers are removed and the number of sampling points is unified. The input to the gripper pose prediction model is the optimized target object point cloud. Task constraint filtering and collision detection are performed on multiple candidate poses inferred by the gripper pose prediction model. The motion trajectory planning adopts a planning strategy that combines LLM trajectory segmentation and fifth-order polynomial velocity smoothing.

[0130] In one embodiment, a robot trajectory planning device for grasping and placing tasks is provided, comprising:

[0131] The task acquisition module is used to acquire task information for capturing and placing tasks;

[0132] The path decomposition module is used to decompose the natural language task instructions based on the large language model to generate a stage sequence as well as stage objectives and / or stage constraints.

[0133] The pose correction module is used to receive the optimal grasping pose and / or placement pose provided by the upstream module, and to replace or correct the stage targets related to grasping and / or placement.

[0134] The trajectory generation and smoothing module is used to generate trajectory segments and perform smoothing processing based on the replaced or corrected stage objectives and / or stage constraints to obtain a trajectory sequence.

[0135] The collision detection and replanning module is used to perform collision detection on the trajectory sequence, and output collision information and perform local repair or replanning when a collision is detected until the collision detection is passed or the preset replanning conditions are met.

[0136] The trajectory output module is used to output the trajectory sequence that the robot controller can execute.

[0137] The specific implementation details of each module can be found in the above description of the limitations of robot trajectory planning methods for grasping and placing tasks, and will not be repeated here.

[0138] The technical features of the above embodiments can be combined in any way. For the sake of brevity, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.

Claims

1. A robot trajectory planning method for grasping and placing tasks, characterized in that, The method includes: Obtain task information for the grabbing and placing task, including natural language task instructions; The natural language task instructions are decomposed based on a large language model to generate a stage sequence and corresponding stage objectives and stage constraints. The stage sequence includes a pre-fetching stage, a fetching stage, a lift stage, a pre-placement stage, a placement stage, and a pullback stage. Receive the optimal grasping pose and / or placement pose provided by the upstream module, and use the optimal grasping pose and / or placement pose to replace or correct the stage targets related to grasping and / or placement in the stage sequence; Trajectory segments are generated based on the replaced or corrected stage objectives and stage constraints, and the trajectory segments are smoothed to obtain a trajectory sequence that meets the controller input requirements. Collision detection is performed on the trajectory sequence; when a collision is detected, collision information is output and local repair or replanning is performed on the trajectory sequence based on the collision information until the collision detection is passed or the preset replanning condition is met; The output robot controller can execute a trajectory sequence, the trajectory sequence including time parameters and pose parameters and / or joint angle parameters corresponding to the time parameters, and including velocity parameters and / or acceleration parameters.

2. The method according to claim 1, characterized in that, The phase sequence, its phase objectives, and phase constraints are output in the form of structured data, which includes phase identifier, phase start point, phase end point, phase attitude constraints, phase height range constraints, and traversed area restrictions.

3. The method according to claim 1, characterized in that, The stage sequence is generated by selecting from a preset set of stage templates, which includes: approach template, alignment template, clamping template, lifting template, transport template, alignment and placement template, release template, and retraction template.

4. The method according to claim 1, characterized in that, The replacement or correction includes: replacing the target pose of the grasping phase and the pre-grasping phase with the optimal grasping pose and its offset pose, and replacing the target pose of the placement phase and the pre-placement phase with the placement pose and its offset pose; wherein the offset pose is used to characterize a transition pose that has a preset spatial offset and / or attitude offset from the optimal grasping pose or placement pose.

5. The method according to claim 1, characterized in that, The trajectory segment generation includes generating the pose trajectory segment of the end effector in Cartesian space and converting the pose trajectory segment into a joint space trajectory segment through inverse kinematics; when there are multiple solutions for inverse kinematics, the joint solution is selected according to the joint limit constraint and / or minimum displacement criterion.

6. The method according to claim 1, characterized in that, The smoothing process includes performing one or more of the following on the trajectory segments: polynomial time parameterization smoothing, spline interpolation smoothing, and / or time scaling based on velocity and acceleration boundaries, to obtain continuous velocity and / or acceleration parameters between segments.

7. The method according to claim 1, characterized in that, The collision detection is based on the following models: robot body geometry model, end effector geometry model, environmental obstacle geometry model, environmental occupancy grid model, and point cloud occupancy region model; the collision detection includes collision determination for discrete sampled poses in the trajectory sequence and / or collision determination for continuous trajectory segments.

8. The method according to claim 1, characterized in that, The collision information includes: the trajectory segment identifier where the collision occurred, the collision sampling point number or time parameter, the collision object identifier, the collision point location, the minimum distance value, and the stage identifier corresponding to the collision.

9. The method according to claim 1, characterized in that, The local repair or replanning includes: adjusting the position of the intermediate key point of the trajectory segment where the collision occurred, adjusting the offset pose of the pre-grabbed or pre-placed segment, adjusting the height parameter of the trajectory segment, adjusting the end-effector attitude constraint, and replacing it with a backup stage template; the preset replanning conditions include at least a maximum replanning number threshold and / or a planning failure alarm condition.

10. A robot trajectory planning device for grasping and placing tasks, characterized in that, include: The task acquisition module is used to acquire task information for capturing and placing tasks; The path decomposition module is used to decompose the natural language task instructions based on the large language model to generate a stage sequence, as well as stage objectives and stage constraints. The pose correction module is used to receive the optimal grasping pose and / or placement pose provided by the upstream module, and to replace or correct the stage targets related to grasping and / or placement. The trajectory generation and smoothing module is used to generate trajectory segments and perform smoothing processing based on the replaced or corrected stage objectives and stage constraints to obtain a trajectory sequence. The collision detection and replanning module is used to perform collision detection on the trajectory sequence, and output collision information and perform local repair or replanning when a collision is detected until the collision detection is passed or the preset replanning conditions are met. The trajectory output module is used to output the trajectory sequence that the robot controller can execute.

Images