Robot motion path planning methods, devices, equipment and storage media
By acquiring operator motion sensing data and multimodal visual data, the system calculates joint motion sequences and understands scene information, generates operational semantic trajectories, and constructs a three-dimensional semantic scene graph model. This solves the adaptability and reliability problems of robot path planning in unstructured environments, and achieves efficient path planning.
Patent Information
- Application Number
- CN202611602785.9
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2026-10-16
- Publication Date
- 2026-11-17
AI Technical Summary
In existing technologies, robots struggle to efficiently utilize environmental information rich in operational guidance in unstructured environments, resulting in insufficient adaptability and reliability in path planning.
By acquiring operator motion sensing data and multimodal visual data, the system calculates joint motion sequences and understands scene information, generates operational semantic trajectories, constructs a 3D semantic scene graph model, and generates robot motion paths to perform operational tasks.
It improves the adaptability and reliability of robot path planning in unstructured scenarios, and can automatically generate motion paths that adapt to environmental changes and meet operational constraints.
Smart Images

Figure CN123103699A_ABST
Abstract
Description
Technical Field
[0001] This application relates to the field of intelligent robot technology, and in particular to robot motion path planning methods, devices, equipment and storage media. Background Technology
[0002] As robot applications extend from structured production lines to unstructured environments, robots need to autonomously complete tasks such as grasping and assembling in real-world scenarios where objects are randomly placed and their layouts are dynamically changing. Their ability to understand the operating environment directly affects the quality of path planning and the success rate of task execution.
[0003] In existing environmental perception technologies, vision-based 3D reconstruction methods can generate point clouds or mesh models of scenes, but the output is only a geometric surface without operational semantics. Although robots can perceive the spatial position and outline of objects, they cannot understand the physical properties of the objects, graspable parts, and avoidance relationships during operations, making it difficult to directly support path planning for fine-grained tasks. To compensate for these shortcomings, one approach relies on manual annotation of key poses and contact events in the operation demonstration video frame by frame. This annotation process is time-consuming and labor-intensive, and the annotation quality varies from person to person. Another approach uses teleoperation devices to drive the robot to reproduce the demonstration actions, but the operator's natural movements are limited by the device structure, and the demonstration trajectory is prone to deviating from real operating habits, making it difficult to guarantee the quality of the collected data.
[0004] Therefore, how to enable robots to efficiently utilize environmental information rich in operational guidance information to improve the adaptability and reliability of path planning in unstructured scenarios is a technical problem that urgently needs to be solved in this field. Summary of the Invention
[0005] The main objective of this application is to provide a robot motion path planning method, apparatus, device, and storage medium, aiming to solve the technical problem in the prior art of how to enable robots to efficiently utilize environmental information rich in operation guidance information in order to improve the adaptability and reliability of path planning in unstructured scenarios.
[0006] To achieve the above objectives, this application provides a robot motion path planning method, which includes the following steps: acquiring motion sensing data and multimodal visual data of an operator performing an operation task demonstration; performing motion calculation based on the motion sensing data to obtain a joint motion sequence of the operator including fingers and limbs; performing scene understanding based on the multimodal visual data to obtain geometric and semantic information of objects in the scene; associating at least one hand trajectory in the joint motion sequence with at least one object in the scene to generate an operation semantic trajectory; constructing a three-dimensional semantic scene graph model based on the operation semantic trajectory and the geometric and semantic information of objects in the scene, wherein the three-dimensional semantic scene graph model includes object nodes, operation pose nodes, and relational edges connecting each node, wherein the node features of object nodes include object attributes, and the node features of operation pose nodes include pose, pose tolerance, and operation constraints; and generating a robot motion path based on the three-dimensional semantic scene graph model to control the robot to perform the operation task.
[0007] Optionally, the acquisition of motion sensing data and multimodal visual data of the operator during the demonstration of the operation task includes: acquiring motion sensing data of the operator during the demonstration of the operation task through a wearable inertial sensing system, wherein the wearable inertial sensing system includes a data glove and / or a full-body motion capture device, and the motion sensing data is MEMS nine-axis inertial sensor data; and acquiring multimodal visual data through multiple RGB-D cameras arranged around the operation scene, wherein the multiple RGB-D cameras are arranged in a surround layout.
[0008] Optionally, the above-mentioned motion calculation based on motion sensing data to obtain the operator's joint motion sequence including fingers and limbs includes: extracting the raw data of the microelectromechanical system inertial sensor from the motion sensing data; performing attitude calculation on the raw data of the microelectromechanical system inertial sensor based on the neural network matrix attitude calculation algorithm to obtain the attitude information of each inertial sensing node of the operator; mapping the attitude information to the three-dimensional position and orientation information of the operator's hand joints and / or the whole body skeletal joints in the global coordinate system according to the biomimetic skeletal kinematic model; and generating the operator's joint motion sequence including fingers and limbs based on the three-dimensional position and orientation information.
[0009] Optionally, the above-mentioned scene understanding based on multimodal visual data to obtain the geometric and semantic information of objects in the scene includes: performing point cloud registration on multi-view color images and depth images in the multimodal visual data to obtain a 3D point cloud of the operating scene; performing object instance segmentation on the 3D point cloud based on a segmentation model to obtain object-level point cloud regions; performing semantic classification on the object-level point cloud regions based on a visual language model to obtain the semantic category of the object, and extracting 3D bounding boxes, surface point cloud features, and physical attributes for each object; and obtaining the geometric and semantic information of each object in the scene based on the semantic category, 3D bounding box, surface point cloud features, and physical attributes.
[0010] Optionally, the above-mentioned association of the hand motion trajectory in the joint motion sequence with at least one object in the scene to generate an operation semantic trajectory includes: calculating the nearest distance between the hand motion trajectory in the joint motion sequence and the point cloud of the object surface in the scene; when the nearest distance is less than or equal to a preset interaction determination threshold, determining that the hand motion trajectory interacts with the object and recording the interaction type; based on a temporal action segmentation network, using the hand motion velocity profile, trajectory curvature, and hand-object distance as features, segmenting the hand motion trajectory into at least one operation primitive, the operation primitive including at least one of approach, grasp, move, align, tighten, and place; encoding the action trajectory corresponding to each operation primitive into a parameterized representation relative to the local coordinate system of the associated object to obtain the operation semantic trajectory.
[0011] Optionally, the above-mentioned construction of a 3D semantic scene graph model based on the semantic trajectory of operations and the geometric and semantic information of objects in the scene includes: constructing an initial graph structure containing object nodes and spatial relationship edges connecting each object node according to the geometric and semantic information of objects in the scene; injecting the semantic trajectory of operations into the initial graph structure, instantiating operation pose nodes, and establishing operation constraint edges between operation pose nodes and object nodes, as well as trajectory sequence edges connecting operation pose nodes in chronological order; performing feature aggregation and relation reasoning on the injected graph structure based on a graph attention network to complete the implicit operation constraints that are not explicitly labeled in the graph structure; and obtaining the 3D semantic scene graph model based on the completed graph structure.
[0012] Optionally, the above-mentioned generation of robot motion paths based on the 3D semantic scene graph model to control the robot to perform operation tasks includes: extracting operation pose node subsequences and object node subgraphs corresponding to the operation tasks from the 3D semantic scene graph model to obtain a planning domain; mapping the poses of the operation pose nodes in the operation pose node subsequences to the equivalent poses of the robot end effector based on a motion redirection strategy; converting the operation semantics in the operation pose nodes from human hand grasping type to the equivalent grasping type of the robot end effector; recalculating the grasping contact point based on the equivalent grasping type; extracting operation constraints from the 3D semantic scene graph model; constructing a constrained trajectory optimization problem based on the planning domain, equivalent pose, grasping contact point, and operation constraints; solving the trajectory optimization problem based on a trajectory parameterization strategy to obtain a robot motion path that satisfies all operation constraints, and controlling the robot to perform operation tasks based on the robot motion path.
[0013] Furthermore, to achieve the above objectives, this application also proposes a robot motion path planning device, which includes: The data acquisition module is used to acquire motion sensor data and multimodal visual data of the operator during the demonstration of the operation task; The motion calculation module is used to calculate motion based on motion sensing data to obtain the operator's joint motion sequence including fingers and limbs; The scene understanding module is used to understand the scene based on multimodal visual data and obtain the geometric and semantic information of objects in the scene; The semantic association module is used to associate at least one hand trajectory in the joint motion sequence with at least one object in the scene to generate an operation semantic trajectory; The model building module is used to construct a 3D semantic scene graph model based on the operation semantic trajectory and the geometric and semantic information of objects in the scene. The 3D semantic scene graph model includes object nodes, operation pose nodes and relational edges connecting each node. Among them, the node features of object nodes include object attributes, and the node features of operation pose nodes include pose and pose tolerance and operation constraints. The path planning module is used to generate robot motion paths based on the 3D semantic scene graph model in order to control the robot to perform operational tasks.
[0014] In addition, to achieve the above objectives, this application also proposes a robot motion path planning device, which includes: a memory, a processor, and a robot motion path planning program stored in the memory and executable on the processor. The robot motion path planning program is configured to implement the steps of the robot motion path planning method described above.
[0015] In addition, to achieve the above objectives, this application also proposes a storage medium storing a robot motion path planning program, which, when executed by a processor, implements the steps of the robot motion path planning method described above.
[0016] The present application proposes one or more technical solutions, which have at least the following technical effects: This method acquires motion sensing data and multimodal visual data from an operator demonstrating an operational task. Since the motion sensing data directly records the operator's actual movement process, the resulting joint motion sequence, including fingers and limbs, can reflect the movement characteristics of natural human operations, providing the robot with a motion reference consistent with operational habits. The multimodal visual data is used for scene understanding, obtaining geometric and semantic information of objects in the scene, enabling the robot to recognize the position, shape, and category of the operated object. The hand trajectory in the joint motion sequence is associated with objects in the scene to generate a trajectory carrying operational semantics. This establishes a correspondence between abstract motion data and specific operational objects, accurately expressing operational intentions. Based on operational semantic trajectories and scene information, a three-dimensional semantic scene graph model is constructed. Since this model includes object nodes, operational pose nodes, and relational edges connecting each node, and the object node features include object attributes, while the operational pose node features include pose, its tolerance, and operational constraints, it can uniformly encode scene geometry, object attributes, and operational constraints, forming a complete planning domain. Finally, the robot's motion path is generated based on this three-dimensional semantic scene graph model, enabling path planning to fully utilize scene structure and operational constraint information, thereby improving the robot's adaptability and reliability in performing operational tasks in unstructured scenes. Attached Figure Description
[0017] The accompanying drawings, which are incorporated in and form part of this specification, illustrate embodiments consistent with this application and, together with the description, serve to explain the principles of this application.
[0018] To more clearly illustrate the technical solutions in the embodiments of this application or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, for those skilled in the art, other drawings can be obtained based on these drawings without creative effort.
[0019] Figure 1 A flowchart illustrating the robot motion path planning method provided in this application embodiment. Figure 1 ; Figure 2 A flowchart illustrating the robot motion path planning method provided in this application embodiment. Figure 2 ; Figure 3A flowchart illustrating the robot motion path planning method provided in this application embodiment. Figure 3 ; Figure 4 This is a schematic diagram of the structure of the robot motion path planning device provided in the embodiments of this application; Figure 5 This is a schematic diagram of the structure of the robot motion path planning device provided in the embodiments of this application. Detailed Implementation
[0020] It should be understood that the specific embodiments described herein are merely illustrative of the technical solutions of this application and are not intended to limit this application.
[0021] To better understand the technical solution of this application, a detailed description will be provided below in conjunction with the accompanying drawings and specific implementation methods.
[0022] As described in the background section, how to enable robots to efficiently utilize environmental information rich in operational guidance information in order to improve the adaptability and reliability of path planning in unstructured scenarios is a technical problem that urgently needs to be solved.
[0023] In view of this, embodiments of this application provide a robot motion path planning method. This method involves linking motion sensing data and multimodal visual data during operator demonstrations to obtain a joint motion sequence. Then, scene understanding is performed on the multimodal visual data to obtain the geometric and semantic information of identifiable objects within the current environment. The hand motion trajectory in the joint motion sequence is associated with the objects to generate an operational semantic trajectory. A semantic scene graph model is constructed based on the scene information. Based on this model, a robot motion path is generated and the corresponding operational task is executed. This invention, by constructing a semantic scene graph model that includes object attributes and operational constraints, enables the robot to understand the operational intent and its association with the scene, and automatically generates a robot motion path that adapts to environmental changes and satisfies operational constraints, thereby improving the flexibility and reliability of the robot in complex path planning.
[0024] The execution subject of this application embodiment can be a robot motion path planning device, or a computing service device with data processing, network communication, and program execution functions, such as a tablet computer, personal computer, or mobile phone, or a thermal management device capable of realizing the above functions of a robot motion path planning device, etc. This embodiment does not specifically limit it. The following uses a robot motion path planning device as the execution subject as an example to describe this embodiment and the following embodiments.
[0025] Reference Figure 1 The robot motion path planning method provided in this application includes steps S10 to S60: Step S10: Acquire motion sensing data and multimodal visual data of the operator during the demonstration of the operation task.
[0026] It should be noted that "operator" refers to a natural person with experience in performing the target task, and the process of demonstrating the task through natural body movements is the source of demonstration data collection. Motion sensing data refers to inertial sensing signals generated during the operator's body movements, collected through wearable sensing devices, specifically including raw measurements such as acceleration, angular velocity, and magnetic field strength. Multimodal visual data refers to scene image information acquired simultaneously from different perspectives by multiple visual sensors, specifically including color images and depth images. The task demonstration refers to the operator's complete execution of the target task according to the actual work process, such as the complete action of taking a wrench from a toolbox and tightening a bolt.
[0027] Understandably, the operator's natural movements during the demonstration accurately reproduce the motion trajectory, speed curve, and operating habits of humans when performing tasks. This results in naturally logical and smooth joint motion sequences for the robot, providing a high-quality motion reference. Multimodal visual data synchronously acquires scene information from multiple perspectives, comprehensively covering the appearance, position, and occlusion relationships of objects in the operating space, providing a solid data foundation for subsequent scene understanding. Motion sensing data and multimodal visual data are acquired synchronously in time, enabling temporal alignment and correlation analysis of motion and scene information, thus accurately reconstructing the interaction between the hand and objects during the operation.
[0028] It should be understood that the acquisition of motion sensing data and multimodal visual data must maintain strict time synchronization. If there is a deviation in the time alignment of the two types of data, misalignment will occur when subsequently associating hand trajectories with objects, affecting the accuracy of the semantic trajectory of the operation. Simultaneously, operators should maintain natural movements during demonstrations and should not deliberately change their operating habits due to wearing sensors. If operators adjust their posture due to wearing the device, the acquired data will deviate from the actual operational characteristics, reducing the reference value for subsequent model construction. Furthermore, the acquisition perspective of multimodal visual data should completely cover the operational space to avoid the omission of some objects in the point cloud data due to viewpoint occlusion.
[0029] In one embodiment, the acquisition of motion sensing data and multimodal visual data of the operator during the demonstration of the operation task includes: acquiring motion sensing data of the operator during the demonstration of the operation task through a wearable inertial sensing system, wherein the wearable inertial sensing system includes a data glove and / or a full-body motion capture device, and the motion sensing data is MEMS nine-axis inertial sensor data; and acquiring multimodal visual data through multiple RGB-D cameras arranged around the operation scene, wherein the multiple RGB-D cameras are arranged in a surround layout.
[0030] It should be noted that wearable inertial sensing systems refer to sensing and data acquisition systems that are worn and fixed to various parts of the operator's body. Data gloves are used to collect motion data of the finger joints, while full-body motion capture devices are used to collect motion data of the limbs and torso. MEMS nine-axis inertial sensor data refers to the raw measurement data output by a sensor that integrates a three-axis accelerometer, a three-axis gyroscope, and a three-axis magnetometer on the same microelectromechanical system chip. An RGB-D camera is a vision sensor that simultaneously outputs color and depth images. Its surround layout refers to multiple cameras evenly distributed in different directions around the operating scene, so that the field of view of each camera covers the operating space from multiple angles.
[0031] Understandably, data gloves can precisely capture the motion information of each finger joint, while full-body motion capture devices can capture the overall motion information of the arm, torso, and lower limbs. The combination of these two allows for the acquisition of multi-level motion data, from fine finger manipulation to coordinated full-body movements, providing a comprehensive data source for subsequent generation of joint motion sequences involving fingers and limbs. The MEMS nine-axis inertial sensor combines the measurement capabilities of an accelerometer, gyroscope, and magnetometer. Fusion of these three types of data enables high-precision attitude calculation. Furthermore, the sensor's small size and low power consumption make it suitable for integration into wearable devices without significantly interfering with the operator's natural movements. Multiple RGB-D cameras arranged in a surround layout can simultaneously acquire color and depth images of the scene from different perspectives, effectively solving the problem of information loss caused by object occlusion from a single viewpoint, providing complete scene data for subsequent point cloud registration and object recognition.
[0032] It should be understood that the sensor nodes of wearable inertial sensing systems must be securely fixed to the corresponding parts of the operator's body when worn. If the sensors become loose or displaced, the acquired attitude data will drift, affecting the accuracy of subsequent motion calculations. The surround layout of RGB-D cameras must ensure that the fields of view of adjacent cameras have a certain overlap area. The overlap area is used for feature matching during multi-view point cloud registration; if the overlap of the fields of view is too small, the registration accuracy will decrease. At the same time, the cameras need to achieve frame-level synchronous acquisition through hardware triggering or time synchronization protocols. If the time synchronization error between cameras is too large, it will cause multi-view images at the same moment to be out of sync in time, affecting the accuracy of subsequent 3D reconstruction and object detection.
[0033] Step S20: Perform motion calculation based on motion sensing data to obtain the operator's joint motion sequence including fingers and limbs.
[0034] It should be noted that motion calculation refers to the process of converting raw inertial data collected by sensors into motion parameters of the operator's skeletal joints. A joint motion sequence is an ordered set of continuously changing three-dimensional position and orientation information of each skeletal joint of the operator over time in a global coordinate system, including finger joints and limb joints. The global coordinate system is a three-dimensional spatial coordinate system established with a fixed reference point in the operating scene (such as a corner point of the workbench or a ground marker) as its origin, used to uniformly describe the spatial position of all objects and joints.
[0035] Understandably, motion sensing data only contains the sensor's own three-dimensional acceleration, angular velocity, and magnetic field strength information, which cannot directly reflect the operator's skeletal posture and joint positions. Therefore, it is necessary to convert the inertial data into the orientation of each sensing node in the global coordinate system through posture calculation, and then map the node orientation to the three-dimensional position and orientation information of the joints based on the skeletal kinematics model. Generating joint motion sequences including fingers and limbs can completely record the motion state of each joint in the hand and limbs during the operation, providing a fine motion data foundation for subsequently associating hand trajectories with scene objects. Compared with solutions that only contain simplified trajectories of the arm or hand, joint motion sequences including fingers and limbs retain richer operational details, such as the degree of fingertip flexion and wrist rotation angle. These details are crucial for distinguishing different operation types and accurately reconstructing the operation process.
[0036] It should be understood that the accuracy of motion calculation directly determines the reliability of joint motion sequences, and the calculation accuracy is affected by factors such as sensor noise, magnetic interference, and the accuracy of the skeletal model. In environments with strong magnetic interference, magnetometer data may produce significant deviations. In such cases, gyroscope and accelerometer data should be fused for calculation to reduce the impact of interference. Furthermore, different operators have different heights, arm lengths, and hand sizes. The parameters of the skeletal kinematic model need to be calibrated according to the individual characteristics of the operator. If the model parameters do not match the operator's actual skeletal dimensions, it will lead to systematic deviations in the calculation of joint positions. In addition, motion calculation should perform a static calibration process after the operator has completed donning the device to establish the initial transformation relationship between the sensor coordinate system and the skeletal coordinate system.
[0037] In one embodiment, the above-mentioned motion calculation based on motion sensing data to obtain the joint motion sequence of the operator including fingers and limbs includes: extracting raw data from the microelectromechanical system inertial sensor in the motion sensing data; performing attitude calculation on the raw data of the microelectromechanical system inertial sensor based on a neural network matrix attitude calculation algorithm to obtain the attitude information of each inertial sensing node of the operator; mapping the attitude information to the three-dimensional position and orientation information of the operator's hand joints and / or the whole body skeletal joints in the global coordinate system according to the biomimetic skeletal kinematics model; and generating the joint motion sequence of the operator including fingers and limbs based on the three-dimensional position and orientation information.
[0038] It should be noted that raw data from MEMS inertial sensors refers to the unprocessed acceleration, angular velocity, and magnetic field strength measurements directly output by the MEMS sensors. The neural network matrix attitude calculation algorithm is an algorithm that uses a deep neural network as its computational core to map raw inertial sensor data into spatial orientation information of sensor nodes, internally representing attitude transformation relationships in matrix form. The biomimetic skeletal kinematic model is a kinematic model constructed based on the structure and joint movement laws of the human skeleton, defining the connections, degrees of freedom, and range of motion between bones. Attitude information refers to the orientation representation of each inertial sensor node in the global coordinate system, usually expressed as quaternions or rotation matrices. Three-dimensional position and orientation information refers to the spatial coordinates of a joint in the global coordinate system and the orientation of the bone segment to which that joint is located.
[0039] Understandably, extracting raw data from inertial sensors first preserves complete motion measurement information for subsequent calculations. Attitude calculation is then performed using a neural network matrix attitude calculation algorithm. Utilizing the training results of deep neural networks on large-scale motion capture data, this effectively fuses data from accelerometers, gyroscopes, and magnetometers, suppressing sensor drift and noise interference to output stable and reliable attitude information. The attitude information is mapped to skeletal joints based on a biomimetic skeletal kinematic model. This process regularizes the calculation results by incorporating the physical connections of the human skeleton and joint degree-of-freedom constraints, ensuring that the output joint positions and orientations conform to human kinematics and avoiding unnatural postures such as reverse bending. The final generated sequence of joint movements, including fingers and limbs, provides time-ordered, complete skeletal motion data for subsequent hand-object association and semantic extraction of manipulation.
[0040] It should be understood that the output accuracy of neural network matrix pose calculation algorithms depends on the coverage of training data and the rationality of the network structure. In practical applications, it is necessary to ensure that the algorithm model has been trained with sufficient motion capture data to adapt to the differences in body shape and movement characteristics of different operators. The length parameters of each bone segment in the biomimetic skeletal kinematic model need to be configured according to the actual body size of the operator. If default parameters are used without adaptation for individual operators, the mapping results of joint positions will be biased. At the same time, attention must be paid to the consistency of coordinate systems during pose calculation. The transformation relationship between the sensor coordinate system, the local coordinate system of the skeleton, and the global coordinate system must be accurately calibrated in advance. Errors in coordinate transformation will directly affect the accuracy of the joint motion sequence.
[0041] Step S30: Perform scene understanding based on multimodal visual data to obtain geometric and semantic information of objects in the scene.
[0042] It should be noted that scene understanding refers to the process of processing multimodal visual data to identify objects and their attributes within a scene. Geometric information refers to the physical geometric attributes of an object, such as its spatial shape, size, position, and orientation, specifically including 3D bounding boxes, surface point clouds, and pose. Semantic information refers to high-level cognitive information such as the object's category, purpose, and physical attributes, specifically including the object's semantic category, material type, mass estimation, and operability labels.
[0043] Understandably, multimodal visual data includes both color and depth images. Color images provide rich color and texture information, suitable for semantic classification of objects; depth images provide spatial distance information of points on the object's surface, suitable for geometric reconstruction. Fusing these two types of data for scene understanding allows for the simultaneous acquisition of both geometric and semantic attributes of objects. This enables the robot to not only know what objects exist in the environment and where they are located, but also their type and physical characteristics, providing data support for subsequently constructing a semantic scene graph model that includes object attributes. Compared to object detection methods based solely on color images, scene understanding incorporating depth information can output precise 3D bounding boxes and surface point clouds of objects, providing necessary information for subsequent hand-object association and grasping planning.
[0044] It should be understood that scene understanding results are affected by the resolution of the visual sensor and ambient lighting conditions. In low-light or highly reflective scenes, the quality of depth images may degrade, leading to reduced accuracy in object segmentation and point cloud reconstruction. Furthermore, occlusion between objects in the scene may prevent some objects from being fully reconstructed; severely occluded objects may only show partial surfaces in the point cloud, necessitating the fusion of multi-view data to fill in the missing areas. In addition, the geometric and semantic information of objects should be stored in a unified descriptive format for easy reference when constructing a semantic scene graph model. Inconsistent information formats will increase the complexity of subsequent data fusion processing.
[0045] In one embodiment, the above-mentioned scene understanding based on multimodal visual data to obtain the geometric and semantic information of objects in the scene includes: performing point cloud registration on multi-view color images and depth images in the multimodal visual data to obtain a three-dimensional point cloud of the operating scene; performing object instance segmentation on the three-dimensional point cloud based on a segmentation model to obtain object-level point cloud regions; performing semantic classification on the object-level point cloud regions based on a visual language model to obtain the semantic category of the object, and extracting three-dimensional bounding boxes, surface point cloud features, and physical attributes for each object; and obtaining the geometric and semantic information of each object in the scene based on the semantic category, three-dimensional bounding boxes, surface point cloud features, and physical attributes.
[0046] It's important to note that point cloud registration refers to the process of unifying independent point cloud data from multiple viewpoints into a single global coordinate system through coordinate transformation, generating a complete scene point cloud. A segmentation model is a deep neural network model used to distinguish and cluster points belonging to different objects in a point cloud, outputting an independent point cloud region for each object. A visual language model is a multimodal understanding model trained jointly on images and text, capable of outputting semantic category descriptions for image or point cloud regions. A 3D bounding box refers to the ability to define the position, size, and orientation parameters of an object's smallest bounding cube. Surface point cloud features refer to descriptors such as the coordinates, normals, and curvature of points on the object's surface. Physical properties refer to parameters such as the object's mass, material type, and surface friction characteristics.
[0047] Understandably, multi-view point cloud registration can fuse independently acquired local point clouds from various cameras into a complete 3D point cloud covering the entire operational scene, eliminating the problem of incomplete object surface information under single-view conditions. Object instance segmentation of the 3D point cloud based on a segmentation model can distinguish point cloud regions belonging to different objects in the scene, laying the foundation for subsequent extraction of geometric and semantic information on an object-by-object basis. Semantic classification of object-level point cloud regions based on a visual language model, utilizing the semantic understanding capabilities obtained through large-scale joint training of images and text, can identify object categories in the scene without additional training for specific object categories, demonstrating good generalization ability. Simultaneously, 3D bounding boxes, surface point cloud features, and physical attributes are extracted, providing comprehensive attribute descriptions for object nodes. This results in object nodes in the subsequently constructed 3D semantic scene graph model possessing rich feature information, supporting the robot's judgment of object operability.
[0048] It should be understood that the accuracy of point cloud registration depends on the accuracy of extrinsic parameter calibration between cameras and the accuracy of point cloud feature matching. If there are errors in the extrinsic parameter calibration, it will lead to ghosting or misalignment in the fused point cloud. Segmentation models may produce inaccurate segmentation for objects with similar shapes or closely adjacent objects. For objects that are in contact or stacked, post-processing is required to further refine the segmentation boundaries. In addition, the classification results of visual language models may have some uncertainty. For the distinction of fine-grained object categories, it is necessary to combine the geometric features of the objects for comprehensive judgment, and avoid relying solely on semantic classification results while ignoring the actual geometric attributes of the objects.
[0049] Step S40: Associate at least one hand trajectory in the joint motion sequence with at least one object in the scene to generate an operation semantic trajectory.
[0050] It should be noted that hand trajectory refers to the motion path formed by the hand joints in a joint motion sequence over time in the global coordinate system, usually represented by a sequence of three-dimensional coordinates of hand keypoints in consecutive time frames. Association refers to the process of determining whether the hand interacts spatially with an object in the scene during its movement, and determining the time and object of the interaction. Operation semantic trajectory refers to the motion trajectory generated after the hand trajectory is associated with an object, containing annotations of the operation object and operation type. It includes not only kinematic information but also semantic information such as "what object was operated on" and "how the object was operated on."
[0051] Understandably, the original hand trajectory only contains kinematic data and cannot reflect which objects the operator touched or what operations were performed during the operation. By associating the hand trajectory with objects in the scene, it is possible to determine which object the hand interacted with at each moment, thus imbuing the motion data with operational semantics. This allows the subsequently constructed 3D semantic scene graph model to express the operational relationships and order between objects. The association process also records the time and type of interaction, providing temporal boundaries and type basis for subsequent segmentation of operation primitives. After generating the operational semantic trajectory, the robot not only knows how the operator's hand moves but also knows which object and what operation each movement corresponds to, achieving the transformation from pure motion data to semantic operational data.
[0052] It should be understood that determining the association between hand trajectories and objects requires setting reasonable interaction criteria. If the criteria are set too strictly, genuine interaction events may be missed; if they are set too leniently, approaching movements that are not in contact with an object may be misjudged as interactions. Furthermore, in scenarios where multiple objects are closely arranged, the hand may be within the vicinity of multiple objects simultaneously. In such cases, it is necessary to combine the hand movement trend with the spatial position of the objects to comprehensively determine the most likely interaction object, avoiding incorrect associations that lead to inaccurate semantic annotations of the operation.
[0053] Step S50: Construct a three-dimensional semantic scene graph model based on the operation semantic trajectory and the geometric and semantic information of objects in the scene.
[0054] It should be noted that the 3D semantic scene graph model is a structured data model that represents objects, operation poses, and their relationships within an operational scene using a graph structure. The graph structure consists of nodes and edges, where nodes represent entities in the scene, and edges represent relationships between entities. Object nodes are graph nodes representing each object in the scene, and their node features include the object's geometric and semantic attributes. Operation pose nodes are graph nodes representing key hand poses during the operation, and their node features include pose parameters as well as the pose's tolerance and operational constraints. Relationship edges are graph edges that connect nodes and express the relationships between them.
[0055] Understandably, the 3D semantic scene graph model encodes object information and operational semantic information obtained from operator demonstrations into a unified graph structure, enabling the static description of the scene and the dynamic process of the operation to be interconnected within the same data model. Object nodes store object attribute information, operation pose nodes store pose and constraint information of the operation process, and relation edges connect the nodes and express spatial relationships and operational constraints. This model includes both static information such as "what objects are in the scene and where they are" and dynamic information such as "how objects should be operated and what constraints exist during the operation process," providing a comprehensive planning domain for subsequent path planning. Compared to storing geometric data and operational data separately, the graph structure can explicitly express the relationships between operation pose nodes and object nodes, allowing the planner to directly use this relationship information to generate motion paths that satisfy operational constraints when searching for paths.
[0056] It should be understood that when constructing a 3D semantic scene graph model, the integrity and consistency of the graph structure must be ensured. All objects appearing in the operational semantic trajectory should have corresponding object nodes in the graph, and all key operational poses should have corresponding operational pose nodes. If there are missing nodes or broken edges in the graph, complete operational information cannot be retrieved during subsequent path planning. Furthermore, the representation of node features and edge features must use a unified data format so that features from different nodes can be jointly calculated and inferred. Inconsistent feature formats will increase the difficulty of graph neural network processing.
[0057] In one embodiment, the above-mentioned construction of a three-dimensional semantic scene graph model based on the semantic trajectory of operations and the geometric and semantic information of objects in the scene includes: constructing an initial graph structure containing object nodes and spatial relationship edges connecting each object node according to the geometric and semantic information of objects in the scene; injecting the semantic trajectory of operations into the initial graph structure, instantiating operation pose nodes, and establishing operation constraint edges between operation pose nodes and object nodes, as well as trajectory sequence edges connecting operation pose nodes in chronological order; performing feature aggregation and relation reasoning on the injected graph structure based on a graph attention network to complete the implicit operation constraints that are not explicitly labeled in the graph structure; and obtaining the three-dimensional semantic scene graph model based on the completed graph structure.
[0058] It should be noted that the initial graph structure refers to the graph structure containing only object nodes and spatial relationship edges before the injection of operational semantics. Spatial relationship edges are edges that express the relative spatial positional relationship between objects, such as "above," "below," "left," and "nested." Injection refers to the process of adding the operational pose nodes and relationship edges from the operational semantic trajectory to the initial graph structure. Instantiation refers to creating the corresponding operational pose nodes based on the pose data in the operational semantic trajectory. Operational constraint edges are edges that connect operational pose nodes to object nodes, expressing the constraint relationship between operational poses and objects. Trajectory sequence edges are directed edges that connect each operational pose node in the order of operation time. Graph attention network refers to a neural network model that aggregates and updates graph node features based on an attention mechanism. Implicit operational constraints refer to constraint relationships in the operational semantic trajectory that are not directly labeled but actually affect the feasibility of the operation.
[0059] Understandably, constructing an initial graph structure containing only object nodes and spatial relationship edges allows for the organization of static geometric and semantic information of the scene into a unified graph representation, providing a foundational framework for subsequent injection of operational semantics. When injecting operational semantic trajectories into the initial graph structure, a corresponding operational pose node is instantiated for each key operational pose, and this node is connected to associated objects via operational constraint edges, establishing a semantic association between operational poses and objects. Trajectory sequence edges connect the operational pose nodes in chronological order, preserving the sequential logic of operational execution within the graph. The graph attention network, through aggregating and reasoning about features between nodes, can automatically learn implicit relationships between nodes and discover constraints that are not explicitly labeled but objectively exist during the operation, such as the avoidance constraint imposed by occlusion of objects below when grasping an object above. The graph structure, after supplementing implicit operational constraints, contains more complete information, providing more comprehensive constraints for subsequent path planning.
[0060] It should be understood that the extraction density of operation pose nodes in the operation semantic trajectory needs to be appropriate. Excessive sampling will result in a large number of nodes in the graph, increasing the inference overhead of the graph neural network; insufficient sampling may miss key constraint information. The inference results of the graph attention network depend on the coverage of the data used during network training. For operation patterns not present in the training data, the implicit constraints of network completion may be uncertain, requiring verification of their rationality in the actual scenario. Furthermore, when injecting the operation semantic trajectory into the initial graph structure, it is necessary to ensure the correct correspondence between operation pose nodes and object nodes. Incorrect associations will cause subsequent planning results to deviate from the intended operation.
[0061] Step S60: Generate the robot motion path based on the 3D semantic scene graph model to control the robot to perform operation tasks.
[0062] It should be noted that the robot motion path refers to the spatial trajectory formed by a series of consecutive pose points traversed by the robot's end effector as it moves from the initial pose to the target pose. This trajectory includes both position and orientation information. Performing the operational task refers to the robot controlling the movement of its joints according to the generated motion path to complete the operational objective consistent with the operator's demonstration.
[0063] Understandably, the 3D semantic scene graph model contains object nodes, operation pose nodes, and their constraint relationships, providing complete input information for path planning. The planner can extract the sequence of operation pose nodes related to the target operation from the graph, obtain the expected pose and operation constraints at each key position, and generate a robot motion path that satisfies both kinematic and operation constraints. Compared with methods that directly plan trajectories without relying on the scene graph model, planning based on the 3D semantic scene graph model can fully utilize the operation constraint information encoded in the model, enabling the generated path to naturally perform operations such as grasping and placing when approaching objects, avoiding problems such as collisions or improper poses.
[0064] It should be understood that the operator and the robot have different morphological and kinematic characteristics. The pose demonstrated by the operator cannot be directly used as the robot's motion path. It needs to be executed after pose mapping and constraint transformation. At the same time, the generated motion path needs to be verified through the robot's kinematic simulation or on an actual robot to ensure that the path meets the robot's joint limits, velocity, acceleration and other constraints.
[0065] In this embodiment, motion sensing data and multimodal visual data are acquired when the operator performs an operation task demonstration. The motion calculation generates a joint motion sequence containing fingers and limbs. The geometric and semantic information of the object is obtained through scene understanding. The hand trajectory is associated with the object to generate an operation semantic trajectory. On this basis, a three-dimensional semantic scene graph model containing object nodes, operation pose nodes and relation edges is constructed. The robot motion path is generated based on the model, realizing the automatic generation of robot motion path from the operator's natural demonstration.
[0066] In summary, this embodiment collects operation data through operator's natural action demonstration, eliminating the need for manual frame-by-frame annotation and remote operation equipment intervention, thus avoiding problems such as high annotation costs and distortion of demonstration actions. By fusing motion sensing data with multimodal visual data, it simultaneously obtains the operator's motion information and the attribute information of scene objects. By constructing a three-dimensional semantic scene graph model that includes object attributes and operation constraints, the robot can understand the operation intention and generate a motion path that adapts to changes in the environment, thereby significantly improving the flexibility and reliability of robot path planning in unstructured scenarios.
[0067] Combination Figure 1Please refer to Figure 2 In the robot motion path planning method provided in this application embodiment, the above step S40 includes steps S401 to S404: Step S401: Calculate the closest distance between the hand motion trajectory in the joint motion sequence and the point cloud of the object surface in the scene.
[0068] It should be noted that the object surface point cloud refers to a set of densely sampled 3D points on the object surface obtained during the scene understanding phase. Each point contains its 3D coordinates in the global coordinate system. The closest distance refers to the minimum spatial distance between a point on the hand's movement trajectory and a point in the object surface point cloud. This distance is used to measure the spatial proximity between the hand and the object.
[0069] Understandably, the closest distance between the hand's movement trajectory and the point cloud of the object's surface is a fundamental quantitative indicator for determining whether the hand has interacted with the object. This distance continuously decreases as the hand approaches the object; it approaches zero when the hand touches the object's surface. By calculating this distance, the hand's movement trajectory can be spatially correlated with objects in the scene, determining which objects the hand approached during its movement. Furthermore, the curve showing the closest distance changing over time also reflects the speed characteristics of the hand approaching the object, serving as an auxiliary criterion for subsequent primitive segmentation operations.
[0070] It should be understood that the hand motion trajectory consists of multiple joints. When calculating the closest distance, representative hand joints (such as the palm or fingertips) must be selected and compared with the object's surface point cloud. Different selections of representative joints will affect the distance calculation results. Furthermore, both the hand motion trajectory and the object's surface point cloud must be in the same global coordinate system for distance calculation to be performed. If their coordinate systems are inconsistent, a coordinate transformation must be performed before distance calculation.
[0071] Step S402: When the nearest distance is less than or equal to the preset interaction judgment threshold, determine that the hand movement trajectory interacts with the object and record the interaction type.
[0072] It should be noted that the preset interaction threshold refers to a pre-set distance threshold used to determine whether a hand has interacted with an object. When the nearest distance is less than or equal to this threshold, it is considered that the hand has interacted with the object. The interaction type refers to the specific way the hand interacts with the object, including different forms such as grasping, touching, pushing, and pressing.
[0073] Understandably, the purpose of setting an interaction threshold is to discretize continuous changes in the nearest distance into explicit interaction events, providing temporal boundaries and type labels for subsequent operation primitive segmentation. When the nearest distance between the hand and the object is less than or equal to the threshold, it indicates that the hand has sufficiently approached the object, resulting in substantial contact or imminent contact, and this is considered an interaction. Recording the interaction type provides semantic annotation for the operation semantic trajectory, ensuring that the trajectory includes not only motion data but also a description of the operation method. For example, in a grasping action, if the distance between the end of the hand's trajectory and the object's surface is less than the threshold, the interaction type is recorded as "grasping," and this information will be passed to subsequent operation semantic trajectories.
[0074] It should be understood that the interaction judgment threshold needs to be set reasonably according to the operation scenario and the size of the object. If the threshold is set too low, brief contact with the object during rapid hand movements may not be recorded; if the threshold is set too high, situations where the hand merely passes by the object without actual contact may be misjudged as interactions. For objects of different sizes, the density of their surface point clouds varies, and the stability of distance calculations also differs. In practical applications, an appropriate judgment threshold needs to be selected by combining the object size and the speed of hand movements.
[0075] Step S403: Based on the temporal action segmentation network, the hand motion trajectory is segmented into at least one operation primitive using the hand motion velocity profile, trajectory curvature, and hand-object distance as features.
[0076] It should be noted that a temporal action segmentation network is a deep learning model capable of detecting action boundaries and identifying action categories on continuous temporal data. Its input is a sequence of temporal features, and its output is an action category label corresponding to each time frame. A hand motion velocity profile is a curve showing the change in hand motion velocity over time, reflecting the speed of hand movements. Trajectory curvature refers to the degree of bending of the hand motion trajectory in space, used to describe the geometric changes in the trajectory. Hand-object distance refers to the spatial distance between the hand and the currently interacting object. Operation primitives are indivisible basic operation units, such as approach, grasp, move, align, tighten, and place.
[0077] Understandably, when an operator performs a task, the entire demonstration process is a continuous sequence of hand movements, containing multiple sub-actions of different types, such as approaching an object, grasping the object, and then moving the object. Different sub-actions have their own characteristics in terms of hand movement speed and trajectory morphology: approaching actions are typically characterized by the hand moving quickly towards the object, with the velocity profile showing an upward trend; grasping actions are typically characterized by the hand's velocity decreasing to a very low level near the object, with a significant change in trajectory curvature; and moving actions are characterized by the hand carrying the object at a relatively stable speed. Using the hand movement velocity profile, trajectory curvature, and hand-object distance as features as input to a temporal action segmentation network enables the network to accurately delineate action boundaries by integrating kinematic and spatial interaction features. After segmenting to obtain operation primitives, the continuous operation demonstration is converted into a discrete sequence of operation units. Each operation primitive has a clear type label and start and end times, laying the foundation for subsequently encoding each operation primitive into a parameterized trajectory.
[0078] It should be understood that the output of temporal action segmentation networks is affected by the coverage of training data. For operation methods or abnormal action patterns that have not been seen during network training, the segmentation results may be biased. Furthermore, the boundaries between operation primitives are sometimes not clearly discernible. For example, the transition from "move" to "align" may be a gradual process. In this case, the boundary of the network output may be segmented at slightly different time positions in different trials, requiring auxiliary judgment based on the abrupt changes in hand-object distance and trajectory curvature. In addition, the duration and amplitude of the same operation primitive may differ in demonstrations by different operators; the network needs to have a certain degree of temporal scale robustness to cope with such variations.
[0079] Step S404: Encode the action trajectory corresponding to each operation primitive into a parameterized representation relative to the local coordinate system of the associated object to obtain the operation semantic trajectory.
[0080] It should be noted that a local coordinate system refers to a coordinate system established with reference to related objects. Typically, the origin is the object's centroid or bounding box center, and the coordinate axes are the directions of the object's principal axes. Parametric representation refers to representing the motion trajectory as a pose sequence relative to the local coordinate system, including position and orientation parameter values.
[0081] Understandably, encoding action trajectories as parameterized representations relative to a local coordinate system of associated objects allows the description of semantic trajectories to be independent of the specific position in the global coordinate system, instead establishing a relative relationship with the operated object. When the object's position changes within the scene, the action trajectory described based on the local coordinate system can still be mapped to the new object position through coordinate transformation, maintaining the universality of the operation method. Simultaneously, the local coordinate system description relative to the object makes the action trajectories of similar objects at different locations comparable, facilitating the reuse of operations on objects at different locations during subsequent path planning. Thus, the semantic action trajectory includes both kinematic parameter information and the relative spatial relationship with the operated object, providing standardized input for subsequently constructing a 3D semantic scene graph model.
[0082] It should be understood that the method for establishing local coordinate systems must be standardized to ensure that the local coordinate systems constructed for the same object in different demonstrations or experiments remain consistent. If the origin or direction of the coordinate system is defined inconsistently, the parametrically represented trajectories will be incomparable. Furthermore, when transforming a trajectory to a local coordinate system, the real-time pose of the associated object in the global coordinate system must be used for coordinate transformation. If there are errors in the object's pose estimation, the transformed trajectory will deviate.
[0083] In this embodiment, the nearest distance between the hand movement trajectory and the point cloud on the object surface is calculated. When the nearest distance is less than or equal to a preset interaction judgment threshold, it is determined that the hand and the object have interacted and the interaction type is recorded. Then, based on the temporal action segmentation network, the hand movement trajectory is segmented into at least one operation primitive using the hand movement velocity profile, trajectory curvature and hand-object distance as features. Finally, the action trajectory corresponding to each operation primitive is encoded into a parameterized representation relative to the local coordinate system of the associated object to obtain the operation semantic trajectory.
[0084] In summary, this embodiment achieves accurate association between hand trajectories and objects through spatial distance calculation and interactive judgment, automatically divides continuous motion sequences into discrete operation primitives through a temporal action segmentation network, and endows operation trajectories with semantic information related to objects through local coordinate system parameterization. Thus, it realizes the automatic generation of operation semantic trajectories that carry operation object and operation type annotations from pure motion trajectories, providing semantically complete input data for the subsequent construction of a three-dimensional semantic scene graph model.
[0085] In combination with the above Figure 1 Please refer to Figure 3 The above step S60 includes steps S601 to S607: Step S601: Based on the operation task, extract the operation pose node subsequence and object node subgraph corresponding to the operation task from the 3D semantic scene graph model to obtain the planning domain.
[0086] It should be noted that the operation task refers to the specific work objective that the robot needs to complete, such as retrieving a specific type of wrench from the toolbox and tightening a bolt. The operation pose node subsequence refers to the sequence of operation pose nodes related to the current operation task in the 3D semantic scene graph model, arranged in chronological order. The object node subgraph refers to a subset of object nodes and their relationships related to the objects involved in the current operation task in the 3D semantic scene graph model. The planning domain refers to the spatial range searched by the path planning algorithm, including the robot's reachable pose space and the obstacle area that needs to be avoided.
[0087] Understandably, a 3D semantic scene graph model contains information about all objects and operation poses in the scene, but the current operation task may only involve a portion of these objects and poses. By extracting relevant operation pose node subsequences and object node subgraphs from the complete scene graph model based on the operation task, the scale of the planning problem can be limited to a task-related range, eliminating interference from irrelevant objects and poses. After extracting the planning domain, the path planning algorithm only needs to search within the planning domain, reducing computational complexity while ensuring that the information used in the planning is directly related to the current task, avoiding false collision detections caused by irrelevant objects.
[0088] It should be understood that the representation of the operation task needs to establish a mapping relationship with the information in the scene graph model. If the task description contains objects or operations that are not modeled in the scene graph, the extraction process will be unable to obtain a complete planning domain. At the same time, the original time order must be maintained when extracting the operation pose node subsequence. This order reflects the logical flow of operation execution. If the order is disrupted, it will lead to incorrect operation order during subsequent path planning.
[0089] Step S602: Based on the motion redirection strategy, map the pose of the operation pose node in the operation pose node subsequence to the equivalent pose of the robot end effector.
[0090] It should be noted that motion retargeting strategy refers to a transformation strategy that maps operator-demonstrated motion data to robot platforms with different kinematic structures. A robot end effector is the component at the end of a robot arm used to perform operations, such as a gripper or suction cup. Equivalent pose refers to the executable pose in the robot coordinate system obtained by transforming the operator's hand pose according to the kinematic characteristics of the robot end effector.
[0091] Understandably, the operator demonstrates using their hand, while the robot performs the operation using an end effector. Significant differences exist between the hand and the end effector in terms of degrees of freedom, link lengths, and joint constraints. Therefore, the operator's hand pose cannot be directly used as the target pose for the robot's end effector. By employing a motion redirection strategy, the operator's hand pose is mapped to an equivalent pose for the robot's end effector. This allows the robot to find the most approachable pose under kinematic constraints that closely matches the operator's demonstration pose, enabling the robot to reproduce the operator's demonstrated operational intent within its own configuration. This mapping process typically requires solving a constrained optimization problem. The optimization objective is to minimize the deviation between the robot's end effector pose and the target pose under candidate configurations, with constraints including joint constraints for each joint.
[0092] It should be understood that motion retargeting strategies must consider the reachable workspace of the robot's end effector. If certain poses demonstrated by the operator are outside the robot's workspace, these poses need to be adjusted or interpolated to bring them within the reachable range. Simultaneously, the retargeting process only changes the spatial position and orientation of the pose representation; the operational semantic information in the operation pose node should remain unchanged to avoid losing operational logic due to retargeting.
[0093] Step S603: Convert the operation semantics in the operation pose node from the human hand grasping type to the equivalent grasping type of the robot end effector.
[0094] It should be noted that operational semantics includes a semantic description of the operational method. The hand grasping type refers to the grasping mode adopted by the hand when performing the operation, such as five-finger envelope grasping, fingertip pinching, or palm pushing. The equivalent grasping type refers to the grasping mode that matches the configuration of the robot's end effector, such as a two-finger gripper using parallel gripping, or a three-finger gripper using central envelope grasping.
[0095] Understandably, the grasping methods used by the operator during the demonstration differ from those achievable by the robot's end effector. The human hand, with its five dexterous fingers, can perform various precise grasping patterns, while a robot's two-finger gripper can only achieve parallel opening and closing, and its three-finger gripper can only achieve limited enveloping gripping. Transforming the operational semantics from human hand grasping to the equivalent grasping type of the robot's end effector allows the robot to complete the same operational goal using its own feasible grasping methods. For example, when converting a human hand's five-finger enveloping grasping to a two-finger gripper's parallel clamping, it's necessary to ensure that the gripper can stably grasp the object with sufficient contact area and clamping force.
[0096] It should be understood that when there are significant differences between the degrees of freedom of a robot end effector and those of a human hand, some human hand grasping types may not have a completely equivalent robot grasping type. In such cases, it is necessary to select the closest feasible grasping method based on the geometric properties of the object and the operational constraints, and to evaluate the impact of this replacement on operational stability.
[0097] Step S604: Recalculate the gripping contact points based on the equivalent gripping type.
[0098] It should be noted that the gripping contact point refers to the location of the point where the robot's end effector makes contact with the object's surface, usually represented by coordinates in the object's local coordinate system. Recalculation refers to re-determining the position of the contact point based on the new gripping type and the object's geometry after a gripping type conversion.
[0099] Understandably, different gripping types correspond to different contact point positions and contact surface shapes. When a human hand grasps with all five fingers, the contact points are distributed on the contact surface between the fingers and the object. However, when a two-finger gripper holds the object, the contact points are located on the opposing surfaces of the fingers and the object. After the gripping type is changed, the contact points calculated based on the human hand are no longer applicable to the robot's end effector. The contact point positions need to be recalculated based on the equivalent gripping type and the object's 3D bounding box and surface point cloud features to ensure that the contact points are located on the object's graspable surface, guaranteeing that the end effector can grasp the object in a stable posture. The recalculation of the gripping contact points also needs to consider the frictional characteristics and structural strength of the object's surface, and the contact points should avoid fragile areas or smooth surfaces.
[0100] It should be understood that when recalculating the grasping contact point, it is necessary to ensure that the contact point is located in the stable grasping area of the object. At the same time, the compatibility between the contact point and the operation pose should also be considered to avoid the robot path obtained in subsequent optimization becoming unexecutable due to changes in the contact point.
[0101] Step S605: Extract operational constraints from the 3D semantic scene graph model.
[0102] It should be noted that operational constraints refer to the restrictions that must be met during the execution of operations, including collision avoidance constraints, force constraints, and pose alignment constraints. Collision avoidance constraints refer to the minimum safe distance that the robot's end effector and joints must maintain from obstacles in the scene during movement. Force constraints refer to the restrictions on the direction and amplitude of contact forces during operation. Pose alignment constraints refer to the relative pose requirements that the tool or end effector and the target object must meet during operation.
[0103] Understandably, the operational constraint edges in a 3D semantic scene graph model contain the constraint relationships between operational pose nodes and object nodes. This constraint information is extracted based on the operator's actual operational behavior during the construction of the operational semantic trajectory. For example, when grasping, the operator needs to maintain a specific relative posture between their hand and the object, and when moving, they need to avoid interference areas from other objects. Extracting operational constraints from the model allows the path planner to incorporate these constraints into the construction of the optimization problem.
[0104] It should be understood that the operational constraints extracted from the 3D semantic scene graph model need to be converted into a form that can be processed by the path planning algorithm. Different types of constraints need to be converted into different mathematical expressions. For example, collision avoidance constraints can be converted into distance inequality constraints, and pose alignment constraints can be converted into upper bound constraints of pose error.
[0105] Step S606: Based on the planning domain, equivalent pose, grasping contact point, and operation constraints, construct a constrained trajectory optimization problem.
[0106] It should be noted that the trajectory optimization problem is a mathematical problem that uses the robot's motion trajectory as the optimization variable, certain performance indicators of the trajectory as the objective function, and operational and kinematic constraints as the constraints, to solve for the optimal trajectory. The objective function typically includes indicators such as trajectory smoothness, execution time, or energy consumption.
[0107] Understandably, integrating the planning domain, equivalent pose, grasping contact point, and operational constraints into the trajectory optimization problem ensures that the resulting optimization problem simultaneously includes task-related path information, robot kinematic information, and operational constraint information. The solution to the trajectory optimization problem is a motion path that satisfies all operational constraints, starts from the initial pose, and sequentially passes through each equivalent pose.
[0108] It should be understood that when constructing a trajectory optimization problem, it is necessary to ensure the feasibility and efficiency of the optimization problem. Too many constraints may lead to an unsolvable optimization problem or excessively long solution time. Therefore, the constraints need to be appropriately simplified and relaxed.
[0109] Step S607: Based on the trajectory parameterization strategy, solve the trajectory optimization problem to obtain the robot motion path that satisfies all operational constraints, and control the robot to perform the operation task according to the robot motion path.
[0110] It should be noted that trajectory parameterization strategy refers to the strategy of expressing the robot's motion trajectory using a set of parameterized basis functions, such as B-splines or polynomials. Solving refers to determining the parameter values in the parameterized trajectory that minimize the objective function and satisfy all constraints using numerical optimization algorithms. The robot's motion path is the continuous trajectory obtained through this solution and followed by the robot during actual execution.
[0111] Understandably, solving the trajectory optimization problem using a trajectory parameterization strategy transforms the infinite-dimensional trajectory planning problem into a finite-dimensional parameter optimization problem, reducing the difficulty of the solution. The resulting robot motion path satisfies the operational constraints implied in the operator's demonstration; by moving along this path, the robot can reproduce the operator's demonstration and complete the operational task.
[0112] It should be understood that the solved robot motion path needs to be post-processed and verified, including collision detection and joint limit checks, to ensure that the path is safe during actual execution. If problems are found during verification, it is necessary to go back and re-optimize or adjust the constraints.
[0113] In this embodiment, the planning domain is obtained by extracting the operation pose node subsequence and object node subgraph corresponding to the operation task from the three-dimensional semantic scene graph model. Based on the motion redirection strategy, the operation pose is mapped to the equivalent pose of the robot end effector. The operation semantics are converted from the human hand grasping type to the equivalent grasping type of the robot end effector. The grasping contact point is recalculated according to the equivalent grasping type. Operation constraints are extracted from the model to construct a constrained trajectory optimization problem.
[0114] In summary, this embodiment transforms the operator's demonstrated pose and grasping semantics into an equivalent form executable by the robot through cross-morphological mapping. By fully utilizing the operational constraint information in the 3D semantic scene graph model through constraint optimization, the generated motion path reproduces the operator's demonstration process, thereby enabling the generalized execution of demonstration operation tasks by robots of different morphologies.
[0115] This application also provides a robot motion path planning device. Please refer to... Figure 4 The robot motion path planning device includes: Data acquisition module 10 is used to acquire motion sensing data and multimodal visual data of the operator when performing the operation task demonstration; The motion calculation module 20 is used to perform motion calculation based on motion sensing data to obtain the joint motion sequence of the operator, including fingers and limbs. The scene understanding module 30 is used to perform scene understanding based on multimodal visual data to obtain geometric and semantic information of objects in the scene; The semantic association module 40 is used to associate at least one hand trajectory in the joint motion sequence with at least one object in the scene to generate an operation semantic trajectory. The model building module 50 is used to build a three-dimensional semantic scene graph model based on the operation semantic trajectory and the geometric and semantic information of objects in the scene. The three-dimensional semantic scene graph model includes object nodes, operation pose nodes and relational edges connecting each node. Among them, the node features of the object node include object attributes, and the node features of the operation pose node include pose and pose tolerance and operation constraints. The path planning module 60 is used to generate robot motion paths based on the three-dimensional semantic scene graph model in order to control the robot to perform operational tasks.
[0116] The robot motion path planning device provided in this application, employing the robot motion path planning method in the above embodiments, can solve the technical problem of how to enable robots to efficiently utilize environmental information rich in operational guidance information, thereby improving the adaptability and reliability of path planning in unstructured scenarios. Compared with the prior art, the beneficial effects of the robot motion path planning device provided in this application are the same as those of the robot motion path planning method provided in the above embodiments, and other technical features in this robot motion path planning device are the same as those disclosed in the methods of the above embodiments, and will not be repeated here.
[0117] This application provides a robot motion path planning device, which includes: at least one processor; and a memory communicatively connected to the at least one processor; wherein the memory stores instructions executable by the at least one processor, and the instructions are executed by the at least one processor to enable the at least one processor to perform the robot motion path planning method in the above embodiment 1.
[0118] The following is for reference. Figure 5 The diagram illustrates a structural schematic of a robot motion path planning device suitable for implementing embodiments of this application. The robot motion path planning device in these embodiments may include, but is not limited to, mobile terminals such as mobile phones, laptops, digital broadcast receivers, PDAs (Personal Digital Assistants), PADs (Portable Application Description), PMPs (Portable Media Players), and in-vehicle terminals (e.g., in-vehicle navigation terminals), as well as fixed terminals such as digital TVs and desktop computers. Figure 5 The robot motion path planning device shown is merely an example and should not impose any limitations on the functionality and scope of use of the embodiments of this application.
[0119] like Figure 5As shown, the robot motion path planning device may include a processing unit 1001 (e.g., a central processing unit, a graphics processing unit, etc.), which can perform various appropriate actions and processes according to a program stored in a read-only memory (ROM) 1002 or a program loaded from a storage device 1003 into a random access memory (RAM) 1004. The RAM 1004 also stores various programs and data required for the operation of the robot motion path planning device. The processing unit 1001, ROM 1002, and RAM 1004 are interconnected via a bus 1005. An input / output (I / O) interface 1006 is also connected to the bus. Typically, the following systems can be connected to the I / O interface 1006: input devices 1007 including, for example, a touchscreen, touchpad, keyboard, mouse, image sensor, microphone, accelerometer, gyroscope, etc.; output devices 1008 including, for example, a liquid crystal display (LCD), speaker, vibrator, etc.; storage devices 1003 including, for example, magnetic tape, hard disk, etc.; and communication devices 1009. The communication device 1009 allows the robot motion path planning device to communicate wirelessly or wiredly with other devices to exchange data. Although the figure shows robot motion path planning devices with various systems, it should be understood that implementation or possession of all the systems shown is not required. More or fewer systems may be implemented alternatively.
[0120] Specifically, according to the embodiments disclosed in this application, the processes described above with reference to the flowcharts can be implemented as computer software programs. For example, embodiments disclosed in this application include a computer program product comprising a computer program carried on a computer-readable medium, the computer program containing program code for performing the methods shown in the flowcharts. In such embodiments, the computer program can be downloaded and installed from a network via a communication device, or installed from storage device 1003, or installed from ROM 1002. When the computer program is executed by processing device 1001, it performs the functions defined in the methods of the embodiments disclosed in this application.
[0121] The robot motion path planning device provided in this application, employing the robot motion path planning method in the above embodiments, can solve the technical problem of how to enable robots to efficiently utilize environmental information rich in operational guidance information, thereby improving the adaptability and reliability of path planning in unstructured scenarios. Compared with the prior art, the beneficial effects of the robot motion path planning device provided in this application are the same as those of the robot motion path planning method provided in the above embodiments, and other technical features in this robot motion path planning device are the same as those disclosed in the previous embodiment method, and will not be repeated here.
[0122] It should be understood that the various parts disclosed in this application can be implemented using hardware, software, firmware, or a combination thereof. In the description of the above embodiments, specific features, structures, materials, or characteristics can be combined in any suitable manner in one or more embodiments or examples.
[0123] The above description is merely a specific embodiment of this application, but the scope of protection of this application is not limited thereto. Any variations or substitutions that can be easily conceived by those skilled in the art within the scope of the technology disclosed in this application should be included within the scope of protection of this application. Therefore, the scope of protection of this application should be determined by the scope of the claims.
[0124] This application provides a computer-readable storage medium having computer-readable program instructions (i.e., a computer program) stored thereon, the computer-readable program instructions being used to execute the robot motion path planning method in the above embodiments.
[0125] The computer-readable storage medium provided in this application may be, for example, a USB flash drive, but is not limited to, electrical, magnetic, optical, electromagnetic, infrared, or semiconductor systems, devices, or any combination thereof. More specific examples of computer-readable storage media may include, but are not limited to: electrical connections having one or more wires, portable computer disks, hard disks, random access memory (RAM), read-only memory (ROM), erasable programmable read-only memory (EPROM or flash memory), optical fiber, portable compact disk read-only memory (CD-ROM), optical storage devices, magnetic storage devices, or any suitable combination thereof. In this embodiment, the computer-readable storage medium may be any tangible medium containing or storing a program that can be used by or in conjunction with an instruction execution system, system, or device. The program code contained on the computer-readable storage medium may be transmitted using any suitable medium, including but not limited to: wires, optical cables, RF (Radio Frequency), etc., or any suitable combination thereof.
[0126] The aforementioned computer-readable storage medium may be included in the robot motion path planning device; or it may exist independently and not be assembled into the robot motion path planning device.
[0127] The aforementioned computer-readable storage medium carries one or more programs. When these programs are executed by the robot motion path planning device, the robot motion path planning device: acquires motion sensing data and multimodal visual data of the operator during the demonstration of the operation task; performs motion calculation based on the motion sensing data to obtain a joint motion sequence of the operator, including fingers and limbs; performs scene understanding based on the multimodal visual data to obtain geometric and semantic information of objects in the scene; associates at least one hand trajectory in the joint motion sequence with at least one object in the scene to generate an operation semantic trajectory; constructs a three-dimensional semantic scene graph model based on the operation semantic trajectory and the geometric and semantic information of the objects in the scene, the three-dimensional semantic scene graph model including object nodes, operation pose nodes, and relational edges connecting the nodes, wherein the node features of the object nodes include object attributes, and the node features of the operation pose nodes include pose and the tolerance and operation constraints of the pose; and generates a robot motion path based on the three-dimensional semantic scene graph model to control the robot to perform the operation task.
[0128] Computer program code for performing the operations of this application can be written in one or more programming languages or a combination thereof, including object-oriented programming languages such as Java, Smalltalk, and C++, and conventional procedural programming languages such as the "C" language or similar programming languages. The program code can be executed entirely on the user's computer, partially on the user's computer, as a standalone software package, partially on the user's computer and partially on a remote computer, or entirely on a remote computer or server. In cases involving remote computers, the remote computer can be connected to the user's computer via any type of network—including a Local Area Network (LAN) or a Wide Area Network (WAN)—or can be connected to an external computer (e.g., via the Internet using an Internet service provider).
[0129] The flowcharts and block diagrams in the accompanying drawings illustrate the architecture, functionality, and operation of possible implementations of systems, methods, and computer program products according to various embodiments of this application. In this regard, each block in a flowchart or block diagram may represent a module, segment, or portion of code containing one or more executable instructions for implementing a specified logical function. It should also be noted that in some alternative implementations, the functions indicated in the blocks may occur in a different order than those indicated in the drawings. For example, two consecutively indicated blocks may actually be executed substantially in parallel, and they may sometimes be executed in reverse order, depending on the functions involved. It should also be noted that each block in the block diagrams and / or flowcharts, and combinations of blocks in the block diagrams and / or flowcharts, can be implemented using a dedicated hardware-based system that performs the specified function or operation, or using a combination of dedicated hardware and computer instructions.
[0130] The modules described in the embodiments of this application can be implemented in software or hardware. The names of the modules do not necessarily limit the functionality of the unit itself.
[0131] The readable storage medium provided in this application is a computer-readable storage medium that stores computer-readable program instructions (i.e., a computer program) for executing the above-described robot motion path planning method. This solves the technical problem of how to enable robots to efficiently utilize environmental information rich in operational guidance information, thereby improving the adaptability and reliability of path planning in unstructured scenarios. Compared with the prior art, the beneficial effects of the computer-readable storage medium provided in this application are the same as those of the robot motion path planning method provided in the above embodiments, and will not be repeated here.
[0132] The computer program product provided in this application can solve the technical problem of robot motion path planning. Compared with the prior art, the beneficial effects of the computer program product provided in this application are the same as the beneficial effects of the robot motion path planning method provided in the above embodiments, and will not be repeated here.
[0133] The above description is only a part of the embodiments of this application and does not limit the patent scope of this application. All equivalent structural transformations made under the technical concept of this application and using the contents of the specification and drawings of this application, or direct / indirect applications in other related technical fields, are included in the patent protection scope of this application.
Claims
1. A robot motion path planning method, characterized in that, The method includes: Acquire motion sensor data and multimodal visual data of the operator during the demonstration of the operation task; Based on the motion sensing data, motion calculation is performed to obtain the joint motion sequence of the operator, including fingers and limbs; Scene understanding is performed based on the multimodal visual data to obtain geometric and semantic information of objects in the scene; Associate at least one hand trajectory in the joint motion sequence with at least one object in the scene to generate an operation semantic trajectory; Based on the operation semantic trajectory and the geometric and semantic information of objects in the scene, a three-dimensional semantic scene graph model is constructed. The three-dimensional semantic scene graph model includes object nodes, operation pose nodes and relational edges connecting each node. The node features of the object nodes include object attributes, and the node features of the operation pose nodes include pose and the tolerance and operation constraints of the pose. The robot's motion path is generated based on the three-dimensional semantic scene graph model to control the robot to perform the operation task.
2. The robot motion path planning method as described in claim 1, characterized in that, The acquisition of motion sensing data and multimodal visual data of the operator during the demonstration of the operation task includes: The motion sensing data of the operator performing the operation task demonstration is obtained by a wearable inertial sensing system, which includes a data glove and / or a full-body motion capture device, and the motion sensing data is MEMS nine-axis inertial sensor data. The multimodal visual data is acquired by multiple RGB-D cameras arranged in a surround layout around the operating scene.
3. The robot motion path planning method as described in claim 1, characterized in that, The step of performing motion calculation based on the motion sensing data to obtain the operator's joint movement sequence including fingers and limbs includes: Extract the raw data from the microelectromechanical system inertial sensor in the motion sensing data; The attitude calculation algorithm of the microelectromechanical system is used to calculate the attitude of the original data of the inertial sensor of the microelectromechanical system, so as to obtain the attitude information of each inertial sensor node of the operator. Based on the biomimetic skeletal kinematics model, the posture information is mapped to the three-dimensional position and orientation information of the operator's hand joints and / or the joints of the whole body in the global coordinate system. Based on the three-dimensional position and orientation information, a sequence of joint movements of the operator's fingers and limbs is generated.
4. The robot motion path planning method as described in claim 1, characterized in that, The step of performing scene understanding based on the multimodal visual data to obtain geometric and semantic information of objects in the scene includes: Point cloud registration is performed on the multi-view color images and depth images in the multimodal visual data to obtain the three-dimensional point cloud of the operation scene; Based on the segmentation model, the 3D point cloud is segmented into object instances to obtain object-level point cloud regions. Based on the visual language model, the point cloud region at the object level is semantically classified to obtain the semantic category of the object, and the 3D bounding box, surface point cloud features and physical attributes are extracted for each object. Based on the semantic category, the 3D bounding box, the surface point cloud features, and the physical properties, the geometric and semantic information of each object in the scene is obtained.
5. The robot motion path planning method as described in claim 1, characterized in that, The step of associating the hand movement trajectory in the joint motion sequence with at least one object in the scene to generate an operation semantic trajectory includes: Calculate the nearest distance between the hand movement trajectory in the joint motion sequence and the point cloud of the object surface in the scene; When the nearest distance is less than or equal to a preset interaction determination threshold, it is determined that the hand movement trajectory interacts with the object, and the interaction type is recorded; Based on a temporal action segmentation network, the hand motion trajectory is segmented into at least one operation primitive, using hand motion velocity profile, trajectory curvature and hand-object distance as features. The operation primitive includes at least one of approach, grasp, move, align, tighten and place. The action trajectory corresponding to each operation primitive is encoded as a parameterized representation relative to the local coordinate system of the associated object to obtain the operation semantic trajectory.
6. The robot motion path planning method as described in claim 1, characterized in that, The construction of a three-dimensional semantic scene graph model based on the operational semantic trajectory and the geometric and semantic information of objects in the scene includes: Based on the geometric and semantic information of the objects in the scene, an initial graph structure is constructed, which includes object nodes and spatial relationship edges connecting each object node. The operation semantic trajectory is injected into the initial graph structure, the operation pose node is instantiated, and operation constraint edges between the operation pose node and the object node are established, as well as trajectory sequence edges connecting the operation pose node in chronological order. Based on the graph attention network, feature aggregation and relation reasoning are performed on the injected graph structure to complete the implicit operation constraints that are not explicitly labeled in the graph structure; Based on the completed graph structure, the three-dimensional semantic scene graph model is obtained.
7. The robot motion path planning method as described in claim 1, characterized in that, The step of generating a robot motion path based on the three-dimensional semantic scene graph model to control the robot to perform the operation task includes: Based on the operation task, extract the operation pose node subsequence and object node subgraph corresponding to the operation task from the three-dimensional semantic scene graph model to obtain the planning domain; Based on the motion redirection strategy, the pose of the operation pose node in the operation pose node subsequence is mapped to the equivalent pose of the robot end effector. The operation semantics in the operation pose node are converted from the human hand grasping type to the equivalent grasping type of the robot end effector; Based on the equivalent grab type, recalculate the grab contact point; Extract operational constraints from the three-dimensional semantic scene graph model; Based on the planning domain, the equivalent pose, the grasping contact point, and the operation constraints, a constrained trajectory optimization problem is constructed. Based on the trajectory parameterization strategy, the trajectory optimization problem is solved to obtain a robot motion path that satisfies all operational constraints, and the robot is controlled to perform the operation task according to the robot motion path.
8. A robot motion path planning device, characterized in that, The device includes: The data acquisition module is used to acquire motion sensor data and multimodal visual data of the operator during the demonstration of the operation task; The motion calculation module is used to perform motion calculation based on the motion sensing data to obtain the joint motion sequence of the operator, including fingers and limbs. The scene understanding module is used to perform scene understanding based on the multimodal visual data to obtain the geometric and semantic information of objects in the scene; A semantic association module is used to associate at least one hand trajectory in the joint motion sequence with at least one object in the scene to generate an operation semantic trajectory; The model building module is used to construct a three-dimensional semantic scene graph model based on the operation semantic trajectory and the geometric and semantic information of objects in the scene. The three-dimensional semantic scene graph model includes object nodes, operation pose nodes and relational edges connecting each node. The node features of the object nodes include object attributes, and the node features of the operation pose nodes include pose and the tolerance and operation constraints of the pose. The path planning module is used to generate a robot motion path based on the three-dimensional semantic scene graph model in order to control the robot to perform the operation task.
9. A robot motion path planning device, characterized in that, The device includes a memory, a processor, and a computer program stored in the memory and running on the processor, the computer program being configured to implement the steps of the robot motion path planning method as described in any one of claims 1 to 7.
10. A computer-readable storage medium, characterized in that, The storage medium stores a computer program, which, when executed by a processor, implements the steps of the robot motion path planning method as described in any one of claims 1 to 7.
