A robot motion generalization method based on topological representation

Abstract

The application discloses a robot motion generalization method based on topological representation, which can effectively encode the topological structure and spatial relationship of the robot grid by gridizing the robot, and comprehensively considers the coupling relationship between each node of the robot, so that the generalization motion obtained by using the application has higher geometric shape similarity with the demonstration motion compared with the existing modeling and training method using the robot end posture, joint angle and the like, and the topological structure and spatial relationship of the demonstration grid are retained. In addition, the proposed motion generalization method based on topological representation has universality. It can be used for the motion generalization task between the same robot systems, and can be used for the motion generalization task between different robot systems; it can realize the motion generalization between single robots, and can realize the motion generalization between multi-robot systems.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of robot skill learning, and in particular to a robot motion generalization method based on topological representation. BACKGROUND

[0002] Motion generalization is a core problem in the field of robot skill learning. Robots are integrated into people's work and life, and humanoid appearance and motion are important factors affecting people's acceptance of robots. For robots, generalizing friendly and humanoid motion is an important skill in human-robot collaboration.

[0003] To achieve humanoid motion generalization, first, the motion obtained by generalization (i.e., the configuration of the robot) should be similar to the demonstrated motion. The motion of the robot is controlled by the combination of each link or joint, so abstracting the coupling relationship between each part of the robot for generalization model training is the key to obtaining a robust generalization result.

[0004] Existing motion generalization methods based on traditional representation methods (such as configuration space, workspace, etc.) can be summarized into two categories: (1) based on motion primitives, (2) based on robot learning. DMP (Dynamic Movement Primitives), KMP (Kernelized Movement Primitives), ProMP (Probabilistic Movement Primitives), BIP (Bayesian Interaction Primitives) and other motion generalization methods based on motion primitives, and motion generalization methods based on machine learning using HMM (Hidden Markov Model), GMM (Gaussian Mixture Model), deep learning, reinforcement learning, etc. On the one hand, when interacting tasks (such as human-robot collaboration, multi-robot collaboration, or considering the relationship between the joints of the robot) are involved, these methods often model each part independently, then achieve association through simple parameter weighting, without explicit representation of the mutual relationship between each part. When training skills, these key features are missing, resulting in an ideal motion generalization result. On the other hand, these methods based on existing representation methods require the demonstration and reproduction of robots to be the same or similar. Due to the differences in joint limits and reachable space of different robots, the generalization model cannot be applied to different robots, otherwise it may cause joint over-limiting or motion unattainable problems. SUMMARY

[0005] The present application aims to overcome the deficiencies of the prior art, and provides a robot motion generalization method based on topological representation, which aims to represent the spatial relationship between each part of the robot and its topological invariance in an alternative way, to obtain a motion more similar to the demonstration by transferring the topological invariance feature of the motion, and to realize the motion generalization between different robots.

[0006] To achieve the above object, the technical scheme provided by the present application is:

[0007] A robot motion generalization method based on topological representation, comprising demonstration data processing and motion generalization.

[0008] The demonstration data processing comprises:

[0009] A1, for each frame of demonstration data, i.e. each state of the demonstration robot, the demonstration robot is nodeized, the demonstration robot is converted into a plurality of nodes, and a node set V is obtained; each node coincides with the coordinate system origin of each link of the demonstration robot and has the same coordinate value as the coordinate system origin;

[0010] A2, the nodes obtained in step A1 are meshed by using the Delaunay triangulation method, and a mesh V is obtained. Wherein E is the edge set of the mesh, and F is the face set of the mesh.

[0011] A3, the spatial relationship between the nodes is coded by using the topological structure of the mesh and the Laplacian matrix, and the state of the demonstration robot is represented by the topological structure of the mesh and the Laplacian matrix.

[0012] The motion generalization comprises:

[0013] B1, the demonstration robot is nodeized in the same way as the demonstration robot, the mesh topological structure of the demonstration robot is transferred to the generalization robot, and the meshing of the generalization robot is completed;

[0014] B2, a deformation energy function of the mesh of the demonstration robot and the mesh of the generalization robot is constructed.

[0015] B3, after obtaining the deformation energy function, deformation optimization is performed.

[0016] B4, by using a joint space mapping method based on the Plucker analysis, a mapping problem from a non-typical workspace to a joint space is solved, and a final generalization result is obtained.

[0017] Further, when the object of motion generalization is a multi-robot system, the nodes of all demonstration robots in the system are regarded as a node of a demonstration robot system, and the meshing is completed by using the Delaunay triangulation method.

[0018] Further, in step A3, for any node i in the grid, its Laplacian coordinate is denoted as: where is the Laplacian operator, is the Cartesian coordinate of node i, N(i) = {j | (i, j) ∈ E} represents the adjacent nodes of node i, ω j is the weight between node i and node j; the Laplacian matrix of all nodes is denoted as:

[0019]

[0020] where is the Cartesian coordinate matrix of all nodes, and n is the number of nodes; L is the random walk normalized Laplacian matrix, and L = I - D -1 A, where I is the identity matrix, D is the degree matrix, and A is the adjacency matrix; the spatial relationship between nodes is coded by using the topology of the grid and the Laplacian matrix, i.e., the state of the robot is represented by the topology of the grid and the Laplacian matrix.

[0021] Further, the deformation energy function of the demonstration robot grid and the generalized robot grid is as follows:

[0022]

[0023] where ||*|| F represents the F-norm.

[0024] Further, the deformation optimization solution includes:

[0025] By keeping the topology of the grid and the spatial relationship between nodes unchanged, i.e., the deformation energy of the demonstration robot grid and the generalized robot grid needs to be minimized, the motion generalization problem is converted into a grid deformation optimization problem, i.e.:

[0026]

[0027] where, is the Cartesian coordinate of all nodes of the generalized robot in the generalization result;

[0028] Then, constraints are added to constrain the deformation optimization process, so as to obtain the following optimization problem:

[0029]

[0030] where m is the number of position constraints, q is the number of rod length constraints, is the relative position matrix describing the nodes that need to be considered for position constraints, and P is the constraint position matrix of the nodes. To describe the relative position matrix of nodes that require length constraints, All elements in the middle are 1. It is necessary to consider the length matrix of the length-constrained edges;

[0031] To simplify the solution, Taylor expansion is used to linearize the quadratic constraints, transforming the problem into a regular QCQP problem:

[0032]

[0033] in, For the Kronecker product, I3 is an identity matrix of dimension n; vec(*) denotes the row vectorization operation on *, v = vec(V) is the row vectorized result of V; δ = vec(Δ), ||*||2 denotes the 2-norm, p = vec(P), v 0 =vec(V d J is the Jacobian matrix of each element in l relative to the Cartesian coordinates of all nodes. 0 This is the length matrix of the edge corresponding to l in the demonstration robot; the Cartesian coordinate vector of the generalized node is obtained by solving formula (6):

[0034]

[0035] Further, step B4 includes:

[0036] B4-1. Robot motion shape representation: Adjacent nodes are connected by straight lines to obtain a spatial polyline consisting of n nodes and n-1 line segments; the mapping problem of joint space is then transformed into: finding a set of joint angle vectors θ such that the spatial polyline l corresponding to θ is aligned with the coordinate vector obtained by the generalization solution. Corresponding spatial polyline The Protodyakonov distance is the smallest;

[0037] B4-2, Parameter Input and Variable Initialization; The parameters and variables that need to be set and initialized are: using the joint angle vector of the demonstration movement as the initial joint angle vector. This represents the initial joint angle of the i-th joint, where N is the number of joints in the robot, and θ is the current iterative joint angle vector. c Initialize to θ 0 The iteration count threshold IT, iteration accuracy threshold IA, iteration step size coefficient IS, non-zero damping coefficient λ, and initial iteration count it = 0; θ is obtained using forward kinematics. c The corresponding coordinate vector v c After shape manipulation, the corresponding spatial polyline l is obtained; For reference, Protodyakonov overlap is performed on l to obtain the transformed coordinate vector v = cIf ′, then the iteration direction vector is: Therefore, the Protodyakonov distance can be calculated as ||x||. If ||x|| ≤ IA or it > IT, the iteration stops, and the desired joint angle vector θ = θ c Otherwise, proceed to the next step;

[0038] B4-3. To control the iteration step size, construct a weighted iteration direction. Therefore, the iterative vector representation of the joint angle is:

[0039]

[0040] Among them, J G For v c Relative to the robot joint angle vector θ c Jacobian matrix; update joint angle = θ c +Δθ, update v c v c ′、x、J G If ||x||≤IA or it>IT, then θ is the desired generalized joint angle, stop the iteration, and output the generalization result; otherwise, continue to execute step B4-3.

[0041] Compared with the prior art, the principles and advantages of the present invention are as follows:

[0042] 1) By meshing the robot, the present invention employs a motion representation method that can effectively encode the topological structure and spatial relationships of the robot mesh. It comprehensively considers the coupling relationships between the robot's nodes. Therefore, compared with the traditional modeling and training methods that use robot end-effector pose, joint angles, etc., the generalized motion obtained by the present invention has a higher geometric similarity to the demonstration motion and retains the topological structure and spatial relationships of the demonstration mesh.

[0043] 2) The motion generalization method based on topological representation proposed in this invention has universality. It can be applied to motion generalization tasks between the same robot system and between different robot systems; it can realize motion generalization between single robots and between multiple robot systems. (1) The joint limits and reachable spaces of different robots are different, which makes it impossible to apply their generalization models to different robots. Otherwise, problems such as joint over-limit or motion unreachability may occur. In the motion generalization process, this invention transfers the topological structure and spatial relationship of the robot mesh. This is a topological and coupling feature that describes the global state of the robot and does not change with the robot. Existing methods only consider the local state of the robot (such as: end pose, joint angle of each joint, etc.). If applied to the generalization task between different robots, problems such as joint over-limit or motion unreachability will likely occur. (2) For the motion generalization problem between multiple robot systems, existing methods cannot be directly used to solve it. This invention treats the nodes of all robots in the system as nodes of a robot system and then uses the Delaunay triangulation method to complete the meshing. The constructed topological representation not only describes the spatial relationships between nodes within each robot, but also the spatial relationships between nodes of different robots, thus completing the state representation of the multi-robot system; by then performing the deformation optimization process in point 5 above, the generalization result can be obtained.

[0044] 3) This invention proposes a joint space mapping method based on Protodyakonov analysis. Through shape manipulation, robot movements are abstracted into spatial geometric polylines. Using the polyline corresponding to the generalization result as a reference, Protodyakonov overlap is performed on the polylines during the iterative solution process. This quantifies the difference between the generalized and iterative movements (Protodyakonov distance) at the same scale, and updates the iteration direction accordingly to obtain the iteration result. By performing Protodyakonov analysis on the generalized and iterative movements and using Protodyakonov distance as a similarity metric, this invention effectively solves the problems of unreasonable measurement and inaccurate mapping results caused by the lack of scale standardization or normalization between the two movement shapes. Therefore, for atypical workspace-to-joint space mapping tasks, compared with existing methods, this invention can more reasonably quantify movement similarity and obtain more accurate mapping results. Attached Figure Description

[0045] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the services required in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings in the following description are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0046] Figure 1This is a flowchart illustrating the principle of a robot motion generalization method based on topological representation according to the present invention.

[0047] Figure 2 This is a schematic diagram of the robot's node-based and mesh-based architecture in this invention;

[0048] Figure 3 This is a schematic diagram of the robot motion generalization process based on topological representation.

[0049] Figure 4 The flowchart shows the joint space mapping method based on Protodyakonov analysis.

[0050] Figure 5 This is a schematic diagram illustrating the shape of robot movements. Detailed Implementation

[0051] The present invention will be further described below with reference to specific embodiments:

[0052] like Figure 1 As shown in the figure, the robot motion generalization method based on topological representation described in this embodiment includes two stages: demonstration data processing and motion generalization.

[0053] The demonstration data processing includes:

[0054] A1. For each frame in the demonstration data, i.e. each state of the demonstration robot, the demonstration robot is nodeified, and the demonstration robot is converted into multiple nodes to obtain a node set V; each node coincides with the origin of the coordinate system of each link of the demonstration robot and has the same coordinate values ​​(relative to the global reference coordinate system).

[0055] A2. Mesh the nodes obtained in step A1 using the Delaunay triangulation method to obtain the mesh. Where E is the edge set of the mesh, and F is the face set of the mesh; when the object of motion generalization is a multi-robot system, it is only necessary to treat the nodes of all robots in the system as nodes of a single robot system, and then use the Delaunay triangulation method to complete the meshing. For example... Figure 2 As shown, where Figure 2 (Left) is a schematic diagram of a single robot in a mesh. Figure 2 (Right) is a schematic diagram of a multi-robot system meshing.

[0056] A3. For any node i within the grid, its Laplace coordinates are expressed as: in For the Laplace operator, Let N(i) = {j|(i,j)∈E} represent the Cartesian coordinates of node i, and let ω be the Cartesian coordinates of node i. jis the weight between node i and node j. The Laplace matrix of all nodes is expressed as:

[0057]

[0058] in Let n be the Cartesian coordinate matrix of all nodes, and n be the number of nodes; L is the Laplace matrix of the random walk normalized matrix, where L = ID. -1 Let A be the grid, where I is the identity matrix, D is the degree matrix, and A is the adjacency matrix. The spatial relationships between nodes are encoded using the grid's topology and the Laplace matrix. The state of the demonstration robot is thus characterized by both the grid's topology and the Laplace matrix.

[0059] Motor generalization includes:

[0060] B1. Perform the same node-based processing on the generalized robot as on the demonstration robot, and transfer the mesh topology of the demonstration robot to the generalized robot, thereby completing the meshing of the generalized robot.

[0061] B2. Construct the deformation energy functions for the demonstration robot mesh and the generalized robot mesh:

[0062]

[0063] in, ||*|| F This represents the F-norm.

[0064] B3. After obtaining the deformation energy function, perform deformation optimization; specifically including:

[0065] By preserving the topology and spatial relationships between nodes, the demonstration robot mesh and the generalized robot mesh remain unchanged. This means minimizing the deformation energy of both meshes, thus transforming the motion generalization problem into a mesh deformation optimization problem.

[0066]

[0067] in, The Cartesian coordinates of all nodes of the generalized robot in the generalization result;

[0068] Next, constraints are added to constrain the deformation optimization process, resulting in the following optimization problem:

[0069]

[0070] Where m is the number of position constraints and q is the number of rod length constraints. To describe the relative position matrix of nodes that need to consider positional constraints, let P be the constraint position matrix of the nodes. To describe the relative position matrix of nodes that require length constraints, All elements in the middle are 1. It is necessary to consider the length matrix of the length-constrained edges;

[0071] The optimization problem constructed above is essentially a quadratic programming problem with equal quadratic constraints, i.e., a non-convex optimization problem. Solving non-convex optimization problems is highly complex, making them unsuitable for tasks with high real-time requirements. To simplify the solution, Taylor expansion is used to linearize the quadratic constraints, transforming the problem into a regular QCQP (quadratically constrained quadratic program) problem:

[0072]

[0073] in, For the Kronecker product, I3 is an identity matrix of dimension n; vec(*) denotes the row vectorization operation on *, v = vec(V) is the row vectorized result of V; δ = vec(Δ), ||*||2 denotes the 2-norm, p = vec(P), v 0 =vec(V d J is the Jacobian matrix of each element in l relative to the Cartesian coordinates of all nodes. 0 This is the length matrix of the edge corresponding to l in the demonstration robot; the Cartesian coordinate vector of the generalized node is obtained by solving formula (6):

[0074]

[0075] B4. Robot control usually requires configuration space, while the solution of formula (6) belongs to a non-typical workspace. (The workspace is composed of the Cartesian coordinates of all nodes), therefore, it is necessary to implement the above workspace. To configuration space The mapping, that is: To address the shortcomings of existing methods based on Euclidean distance, which cannot accurately measure the similarity between two actions and thus may affect the accuracy of joint space mapping results, this invention proposes a joint space mapping method based on Protodyakonov analysis. This method finds a standard shape through Protodyakonov overlap and uses Protodyakonov distance as a similarity metric function for actions, thereby accurately solving the mapping problem from atypical workspaces to joint spaces. The flowchart is as follows. Figure 4 As shown, the details are as follows:

[0076] B4-1. Robot motion shape representation: Adjacent nodes are connected by straight lines to form a spatial polyline consisting of n nodes and n-1 line segments, such as... Figure 5As shown, the mapping problem in joint space is transformed into: finding a set of joint angle vectors θ such that the spatial polyline l corresponding to θ intersects the coordinate vector obtained by the generalization solution. Corresponding spatial polyline The Protodyakonov distance is the smallest;

[0077] B4-2, Parameter Input and Variable Initialization; The parameters and variables that need to be set and initialized are: using the joint angle vector of the demonstration movement as the initial joint angle vector. This represents the initial joint angle of the i-th joint, where N is the number of joints in the robot, and θ is the current iterative joint angle vector. c Initialize to θ 0 The iteration count threshold IT, iteration accuracy threshold IA, iteration step size coefficient IS, non-zero damping coefficient λ, and initial iteration count it = 0; θ is obtained using forward kinematics. c The corresponding coordinate vector v c After shape manipulation, the corresponding spatial polyline l is obtained; For reference, Protodyakonov overlap is performed on l to obtain the transformed coordinate vector v. c If ′, then the iteration direction vector is: Therefore, the Protodyakonov distance can be calculated as ||x||. If ||x|| ≤ IA or it > IT, the iteration stops, and the desired joint angle vector θ = θ c Otherwise, proceed to the next step;

[0078] B4-3. To control the iteration step size, construct a weighted iteration direction. Therefore, the iterative vector representation of the joint angle is:

[0079]

[0080] Among them, J G For v c Relative to the robot joint angle vector θ c Jacobian matrix; update joint angle = θ c +Δθ, update v c v c ′、x、J G If ||x|≤IA or it>IT, then θ is the desired generalized joint angle, stop the iteration, and output the generalization result; otherwise, continue to execute step B4-3.

[0081] The above-described embodiments are merely preferred embodiments of the present invention and are not intended to limit the scope of the present invention. Therefore, any changes made in accordance with the shape and principle of the present invention should be covered within the protection scope of the present invention.

Claims

1. A robot motion generalization method based on topological representation, characterized in that, Includes demonstration data processing and motion generalization; The demonstration data processing includes: A1. For each frame in the demonstration data, i.e. each state of the demonstration robot, the demonstration robot is nodeified and converted into multiple nodes to obtain a node set V; each node coincides with the origin of the coordinate system of each link of the demonstration robot and has the same coordinate values ​​as it. A2. Use the Delaunay triangulation method to mesh the nodes obtained in step A1 to obtain the mesh M = (V, E, F), where E is the edge set of the mesh and F is the face set of the mesh. A3. The spatial relationships between nodes are encoded using the topology of the mesh and the Laplace matrix. The state of the demonstration robot is characterized by the mesh topology and the Laplace matrix. Motor generalization includes: B1. Perform the same node-based processing on the generalized robot as on the demonstration robot, and transfer the mesh topology of the demonstration robot to the generalized robot, thereby completing the meshing of the generalized robot. B2. Construct the deformation energy functions for the demonstration robot mesh and the generalized robot mesh; B3. After obtaining the deformation energy function, perform deformation optimization solution; B4. Using the joint space mapping method of Protodyakonov analysis, solve the mapping problem from atypical workspace to joint space and obtain the final generalization result.

2. The robot motion generalization method based on topological representation according to claim 1, characterized in that, When the object of motion generalization is a multi-robot system, the nodes of all the demonstration robots in the system are treated as nodes of a single demonstration robot system, and then the Delaunay triangulation method is used to complete the meshing.

3. The robot motion generalization method based on topological representation according to claim 1, characterized in that, In step A3, for any node i within the mesh, its Laplace coordinates are expressed as: in For the Laplace operator, Let N(i) = {j | (i,j) ∈ E} represent the Cartesian coordinates of node i, and let ω be the Cartesian coordinates of node i. j The weights between node i and node j are given by: The Laplace matrix of all nodes is represented as: in Let n be the Cartesian coordinate matrix of all nodes, and n be the number of nodes; L is the Laplace matrix of the random walk normalized matrix, where L = ID. -1 A, where I is the identity matrix, D is the degree matrix, and A is the adjacency matrix; the spatial relationships between nodes are encoded using the topology of the mesh and the Laplace matrix, that is, the state of the robot is jointly represented by the topology of the mesh and the Laplace matrix.

4. The robot motion generalization method based on topological representation according to claim 3, characterized in that, The deformation energy functions of the constructed demonstration robot mesh and generalized robot mesh are as follows: in, ||*|| F This represents the F-norm.

5. The robot motion generalization method based on topological representation according to claim 4, characterized in that, Perform deformation optimization solutions, including: By preserving the topology and spatial relationships between nodes, the demonstration robot mesh and the generalized robot mesh remain unchanged. This means minimizing the deformation energy of both meshes, thus transforming the motion generalization problem into a mesh deformation optimization problem. in, The Cartesian coordinates of all nodes of the generalized robot in the generalization result; Next, constraints are added to constrain the deformation optimization process, resulting in the following optimization problem: Where m is the number of position constraints and q is the number of rod length constraints. To describe the relative position matrix of nodes that need to consider positional constraints, let P be the constraint position matrix of the nodes. To describe the relative position matrix of nodes that require length constraints, All elements in the middle are 1. It is necessary to consider the length matrix of the length-constrained edges; To simplify the solution, Taylor expansion is used to linearize the quadratic constraints, transforming the problem into a regular QCQP problem: in, For the Kronecker product, I3 is an identity matrix of dimension n; vec(*) denotes the row vectorization operation on *, v = vec(V) is the row vectorized result of V; δ = vec(Δ), ||*||2 denotes the 2-norm, p = vec(P), v 0 =vec(V d J is the Jacobian matrix of each element in l relative to the Cartesian coordinates of all nodes. 0 This is the length matrix of the edge corresponding to l in the demonstration robot; the Cartesian coordinate vector of the generalized node is obtained by solving formula (6):

6. The robot motion generalization method based on topological representation according to claim 5, characterized in that, Step B4 includes: B4-1. Robot motion shape representation: Adjacent nodes are connected by straight lines to obtain a spatial polyline consisting of n nodes and n-1 line segments; the mapping problem of joint space is then transformed into: finding a set of joint angle vectors θ such that the spatial polyline l corresponding to θ is aligned with the coordinate vector obtained by the generalization solution. Corresponding spatial polyline The Protodyakonov distance is the smallest; B4-2, Parameter Input and Variable Initialization; The parameters and variables that need to be set and initialized are: using the joint angle vector of the demonstration movement as the initial joint angle vector. This represents the initial joint angle of the i-th joint, where N is the number of joints in the robot, and θ is the current iterative joint angle vector. c Initialize to θ 0 The iteration count threshold IT, iteration accuracy threshold IA, iteration step size coefficient IS, non-zero damping coefficient λ, and initial iteration count it = 0; θ is obtained using forward kinematics. c The corresponding coordinate vector v c After shape manipulation, the corresponding spatial polyline l is obtained; For reference, Protodyakonov overlap is performed on l to obtain the transformed coordinate vector v. c If ′, then the iteration direction vector is: Therefore, the Protodyakonov distance can be calculated as ||x||. If ||x|| ≤ IA or it > IT, the iteration stops, and the desired joint angle vector θ = θ c Otherwise, proceed to the next step; B4-3. To control the iteration step size, construct a weighted iteration direction. Therefore, the iterative vector representation of the joint angle is: Among them, J G For v c Relative to the robot joint angle vector θ c Jacobian matrix; update joint angle θ = θ c +Δθ, update v c v c ′、x、J G If ||x||≤IA or it>IT, then θ is the desired generalized joint angle, stop the iteration, and output the generalization result; otherwise, continue to execute step B4-3.

Images