A robot rapid motion planning method and system

Abstract

This invention relates to a rapid robot motion planning method. First, it models example actions using a sequence of trajectory points that characterizes task features. Then, it extracts key points from the example trajectory using a trajectory segmentation method based on a first-order difference metric. Finally, it uses PCA to obtain the motion pattern features of the key points of the example trajectory, determines prediction constraints, and achieves rapid motion prediction under a given robot state by solving a quadratic programming problem under these constraints, thus completing the planning. This invention also provides a rapid robot motion planning system. This invention improves the efficiency of robot motion generation in industrial environments and enhances the robot's adaptability to real-time customized tasks.

Description

Technical Field

[0001] This invention belongs to the field of intelligent manufacturing and relates to a method and system for rapid motion planning of robots. Background Technology

[0002] In practical robot assembly scenarios, robot motion trajectories need to be designed and planned to enable robots to perform predetermined tasks. For industrial robots, motion planning means planning the robot's displacement, velocity, and acceleration during movement to obtain stable and reasonable end effector motion trajectories. However, in variable and customized assembly tasks, designing robot trajectories often relies heavily on expert experience and lacks good versatility. Once the task changes, experts need to reprogram the robot. Industrial robots in intelligent manufacturing need to have the flexibility to perform multiple tasks simultaneously. The method of manually setting trajectories relies too heavily on expert programming experience, resulting in low efficiency in adapting to new tasks. This makes robot motion planning one of the limiting factors for the rapid deployment of customized tasks in industrial applications.

[0003] Besides experts directly setting robot actions, there are now more intelligent robot motion planning methods, which can be broadly summarized as motion planning methods based on dynamic motion primitives, motion planning methods based on reinforcement learning, and motion planning methods based on imitation learning. Among these, methods based on dynamic motion primitives are difficult to model and complex to solve; methods based on reinforcement learning suffer from time-consuming training, poor stability, and can also disrupt the training environment; imitation learning refers to learning from examples provided by a teacher, generally learning the optimal strategy from decision data from human experts. Methods based on imitation learning combine speed and flexibility in motion planning, making them more suitable for the rapid adaptation of customized assembly tasks for industrial robots.

[0004] Currently, robot imitation learning has the following shortcomings:

[0005] 1. Insufficient abstraction of robot actions leads to weak generalization, making it difficult to solve problems well after changes in the robot's initial state;

[0006] 2. The teaching data is mostly images and other data, lacking three-dimensional spatial information and having low accuracy;

[0007] 3. Imitation learning mostly uses neural network-based methods, which lack interpretability. Summary of the Invention

[0008] The technical problem solved by this invention is to overcome the shortcomings of the prior art and propose a method and system for rapid robot motion planning, thereby improving the efficiency of robot motion generation in industrial environments and enhancing the robot's adaptability to real-time customized tasks.

[0009] The solution of the present invention is:

[0010] A method for rapid motion planning of a robot includes:

[0011] Model example actions using trajectory point sequences that can characterize task features;

[0012] Key points of example trajectories are extracted using a trajectory segmentation method based on first-order difference metric.

[0013] PCA is used to obtain the motion pattern features of key points of example trajectories, and prediction constraints are determined. By solving the quadratic programming problem under the constraints, fast motion prediction under a given robot state is achieved, thus completing the planning.

[0014] Preferably, when modeling example actions using a sequence of trajectory points that can characterize task features, it is assumed that the example trajectory for a specific task is a sequence traj of trajectory point information obtained through uniform sampling, where...

[0015] traj={initpoint,point1,point2,…,point i ...point n}

[0016] point i =[x i y i , z i a i b i c i d i ] T

[0017] Where initpoint represents the initial trajectory point information, point i x represents the information of the i-th trajectory point. i y i , z i Let a represent the Cartesian coordinates of the i-th trajectory point in the workspace. i b i c i d i Let represent the quaternion of the i-th trajectory point in the workspace.

[0018] Preferably, a trajectory segmentation method based on first-order difference metric is used to extract key points of the example trajectory. The specific implementation method is as follows:

[0019] (1) Given example trajectory data traj = {initpoint, point1, ..., point2} containing n+1 pose points, the following conditions are met: n}, then the first-order difference component is defined as the difference between adjacent trajectory points, and the first-order difference component sequence Δtraj satisfies Δtraj=point1-initpoint, point2-point1, …, point i -point i-1 ...point n -point n-1

[0020] Wherein, the i-th first-order difference component satisfies

[0021]

[0022] (2) Based on the first-order difference component sequence, all the segmentation methods are traversed to find k optimal segmentation points on the example trajectory. The k optimal segmentation points divide the example trajectory data into k+1 segments, and the sum of the variances of the first-order difference components of each segment of trajectory data is minimized.

[0023] (3) The optimal segmentation point is the key point on the example trajectory.

[0024] Preferably, the implementation method for determining the prediction constraints by using PCA to obtain the action pattern features of key points in the example trajectory is as follows:

[0025] The solution space of the quadratic programming problem is defined in a 7×k-dimensional vector space, with the independent variable being the 7×k-dimensional key point vector X:

[0026] X=[x1,y1,z1,a1,b1,c1,d1,…,x k y k , z k a k b k c k d k ] T

[0027] PCA transformation is expressed as the formula

[0028] Y = A T X

[0029] in,

[0030] Y = [y1, y2, ..., y] 7k ] T

[0031] X and Y are both 7×k dimensional column vectors, A T The transformation matrix is ​​(7×k)×(7×k);

[0032] Given a set of m data

[0033] X = [X1, X2, ..., X] m ] p×m

[0034] p represents the dimension of the keypoint vector for each example trajectory, p = 7 × k; m represents the number of example trajectories;

[0035] Data obtained after PCA transformation

[0036] Y = [Y1, Y2, ..., Y] m ] p×m

[0037] Let Y be the element in the i′ row and j′ column of matrix Y. i′j′ Let be the value of the i′th dimension of the j′th data point.

[0038]

[0039] Therefore, for the dimensions corresponding to the q principal components of the Y matrix, the following inequality constraints are constructed.

[0040]

[0041]

[0042] A represents T The first q lines;

[0043] For the remaining pq dimensions of the Y matrix, since the variance after PCA is small, we have

[0044]

[0045] Therefore, for these pq-dimensional features, the following equality constraints are constructed.

[0046]

[0047] A represents T The last pq row, the right side of the equation represents the expectation of each dimension of the pq-dimensional nonprincipal components after PCA transformation;

[0048] The inequality constraints and equality constraints mentioned above are the predictive constraints.

[0049] Preferably, rapid action prediction under a given robot state is achieved by solving a quadratic programming problem under constraints, as follows:

[0050] The optimization objective tar of the quadratic programming problem is defined as the sum of the squares of the Euclidean distances between adjacent key points, expressed as:

[0051]

[0052] Where k represents the total number of key points, [x i y i , z i ] T Let [x0, y0, z0] represent the Cartesian coordinates of the i-th key point. T This represents the Cartesian coordinates of the robot's end effector gripper in its initial state;

[0053] Therefore, the optimization objective tar is a quadratic function of the independent variable, represented in matrix form.

[0054] tar=X T HX+gX+ε

[0055] Where X represents the independent variable, H represents the quadratic term matrix, g represents the linear term matrix, and ε represents the constant term in the optimization objective;

[0056] Find the matrix H corresponding to different values ​​of k. Matrix H is an upper triangular matrix, and the diagonal elements only take the three values ​​0, 1, and 2. Therefore, H is a positive semi-definite matrix. Thus, the optimization objective tar is a convex quadratic function.

[0057] By using the mature convex quadratic programming method, the prediction of the robot's rapid actions under a given initial state can be obtained.

[0058] A robot rapid motion planning system includes: an example motion modeling module, a key point extraction module, and a motion prediction module;

[0059] Example Action Modeling Module: Models example actions using a sequence of trajectory points that can characterize task features;

[0060] Key point extraction module: The key points of the example trajectory are extracted using a trajectory segmentation method based on first-order difference metric;

[0061] Action prediction module: PCA is used to obtain the action pattern features of key points of example trajectory, the prediction constraints are determined, and the rapid action prediction under a given robot state is achieved by solving the quadratic programming problem under the constraints, thus completing the planning.

[0062] Preferably, when the example action modeling module models the example action using a sequence of trajectory points that can characterize task features, it assumes that the example trajectory for a specific task is a sequence of trajectory point information traj obtained by uniform sampling, where...

[0063] traj={initpo int, point1, point2,…, pointi,…point n}

[0064] point i =[x i y i , z i a i b i c i d i ] T

[0065] Where initpoint represents the initial trajectory point information, point i x represents the information of the i-th trajectory point. i y i , z i Let a represent the Cartesian coordinates of the i-th trajectory point in the workspace. i b i c i d i Let represent the quaternion of the i-th trajectory point in the workspace.

[0066] The preferred implementation method for the key point extraction module is as follows:

[0067] (1) Given example trajectory data traj = {initpoint, point1, ..., point2} containing n+1 pose points, the following conditions are met: n}, then the first-order difference component is defined as the difference between adjacent trajectory points, and the first-order difference component sequence Δtraj satisfies Δtraj=point1-initpoint, point2-point1, …, point i -point i-1 ...point n -point n-1

[0068] Wherein, the i-th first-order difference component satisfies

[0069]

[0070] (2) Based on the first-order difference component sequence, all the segmentation methods are traversed to find k optimal segmentation points on the example trajectory. The k optimal segmentation points divide the example trajectory data into k+1 segments, and the sum of the variances of the first-order difference components of each segment of trajectory data is minimized.

[0071] (3) The optimal segmentation point is the key point on the example trajectory.

[0072] Preferably, the action prediction module uses PCA to obtain the action pattern features of key points in the example trajectory, and the implementation method for determining the prediction constraints is as follows:

[0073] The solution space of the quadratic programming problem is defined in a 7×k-dimensional vector space, with the independent variable being the 7×k-dimensional key point vector X:

[0074] X=[x1,y1,z1,a1,b1,c1,d1,…,x k y k , z k a k b k c k d k ] T

[0075] PCA transformation is expressed as the formula

[0076] Y = A T X

[0077] in,

[0078] Y = [y1, y2, ..., y] 7k ] T

[0079] X and Y are both 7×k dimensional column vectors, A T The transformation matrix is ​​(7×k)×(7×k);

[0080] Given a set of m data

[0081] X = [X1, X2, ..., X] m ] p×m

[0082] p represents the dimension of the keypoint vector for each example trajectory, p = 7 × k; m represents the number of example trajectories;

[0083] Data obtained after PCA transformation

[0084] Y = [Y1, Y2, ..., Y] m ] p×m

[0085] Let Y be the element in the i′ row and j′ column of matrix Y. i′j′ Let be the value of the i′th dimension of the j′th data point.

[0086]

[0087] Therefore, for the dimensions corresponding to the q principal components of the Y matrix, the following inequality constraints are constructed.

[0088]

[0089]

[0090] A represents T The first q lines;

[0091] For the remaining pq dimensions of the Y matrix, since the variance after PCA is small, we have

[0092]

[0093] Therefore, for these pq-dimensional features, the following equality constraints are constructed.

[0094]

[0095] A represents T The last pq row, the right side of the equation represents the expectation of each dimension of the pq-dimensional nonprincipal components after PCA transformation;

[0096] The inequality constraints and equality constraints mentioned above are the predictive constraints.

[0097] Preferably, the action prediction module achieves rapid action prediction for a given robot state by solving a quadratic programming problem under constraints, as follows:

[0098] The optimization objective tar of the quadratic programming problem is defined as the sum of the squares of the Euclidean distances between adjacent key points, expressed as:

[0099]

[0100] Where k represents the total number of key points, [x i y i , z i ] T Let [x0, y0, z0] represent the Cartesian coordinates of the i-th key point. T This represents the Cartesian coordinates of the robot's end effector gripper in its initial state;

[0101] Therefore, the optimization objective tar is a quadratic function of the independent variable, represented in matrix form.

[0102] tar=X T HX+gX+ε

[0103] Where X represents the independent variable, H represents the quadratic term matrix, g represents the linear term matrix, and ε represents the constant term in the optimization objective;

[0104] Find the matrix H corresponding to different values ​​of k. Matrix H is an upper triangular matrix, and the diagonal elements only take the three values ​​0, 1, and 2. Therefore, H is a positive semi-definite matrix. Thus, the optimization objective tar is a convex quadratic function.

[0105] By using the mature convex quadratic programming method, the prediction of the robot's rapid actions under a given initial state can be obtained.

[0106] The beneficial effects of this invention compared to the prior art are:

[0107] (1) The robot motion trajectory point modeling method proposed in this invention can reasonably abstract the robot motion trajectory, abstract the robot motion into several key points, and improve the generalization of the motion; combined with traditional interpolation and other planning methods, it can accurately represent specific motion trajectories with high accuracy and simplified and efficient modeling.

[0108] (2) The trajectory segmentation method based on first-order difference metric of this invention directly uses the trajectory as an example. It has high accuracy and can use the velocity attribute of the trajectory for trajectory segmentation. It only needs to set the number of trajectory segments according to the action without any additional settings. Its accuracy and execution efficiency can meet the requirements of rapid adaptation to customized tasks in industrial scenarios.

[0109] (3) This invention defines the action pattern as the correlation and value range between different dimensions of the trajectory key points, and abstracts the pattern through PCA, which is highly interpretable. The key point prediction method based on PCA and quadratic programming transforms the robot motion planning method into a constrained optimization problem. "Constraint" represents the action features, and "optimization" improves the simplicity of the trajectory and the execution speed. It can predict key points based on the action features learned from the teaching data, and its prediction accuracy is high, which can meet the requirements of motion planning in industrial scenarios. Attached Figure Description

[0110] Figure 1 The movement trajectory of the container during the liquid pouring task is shown, where (a) is the starting point, (b) is key point 1, and (c) is key point 2.

[0111] Figure 2 A schematic diagram of the world coordinate system and the object coordinate system;

[0112] Figure 3 This is a schematic diagram of the PCA transformation result. Detailed Implementation

[0113] The invention will now be further described with reference to the accompanying drawings.

[0114] This invention aims to address the problems of low adaptation efficiency and flexibility in existing robot motion planning by establishing a rapid robot motion planning method and system. This method eliminates the need for manual programming to set robot motions; instead, on-site operators simply need to execute the corresponding robot actions using VR controllers. The robot then actively learns the motion pattern characteristics, thereby efficiently and rapidly adapting to new tasks.

[0115] This invention aims to accomplish the following:

[0116] 1. Robot motion abstraction and modeling

[0117] In order to make full use of mature controller and inverse kinematics solver theories, and in order to adopt a more generalized method to represent robot action patterns, this invention studies a more concise robot action representation method based on abstract action features, and uses trajectory point sequences that can represent task features to model example actions.

[0118] 2. Trajectory segmentation method based on first-order difference metric

[0119] Based on modeling robot motion as k key points, in order to learn features for predicting key points from example trajectories, the key points should first be extracted from the example trajectories. This invention designs a trajectory segmentation method based on first-order difference metric for extracting key points from example trajectories.

[0120] 3. Keypoint Prediction Method Based on PCA and Quadratic Programming

[0121] After extracting key points from the example trajectory, action pattern features need to be learned from these key points, and actions need to be predicted under a given robot state. How to predict actions under a given robot state so that the predicted actions conform to the action pattern features contained in the example trajectory data, while also making the trajectory as concise as possible, is also the problem addressed in this invention. In this step, PCA is used to obtain the action pattern features of the key points of the example trajectory, prediction constraints are determined, and fast action prediction under a given robot state is achieved by solving a quadratic programming problem under the constraints, thus completing the planning.

[0122] The ideas in this invention can be divided into three parts: "robot motion key point modeling strategy", "trajectory segmentation method based on first-order difference metric", and "key point prediction method based on PCA and quadratic programming".

[0123] (1) Robot motion key point modeling strategy

[0124] This invention models robot motion as a set of k key points.

[0125] Figure 1 The diagram illustrates the movement trajectory of a container during a liquid-pouring task. In this task, the robotic arm needs to grasp container A containing liquid, move it near a target container B, and then inject the liquid into container B. This invention proposes that robot actions can be modeled using a few trajectory points that represent key features of the action, while other trajectory points can be left unmodeled. For the liquid-pouring task, given an initial state, such as... Figure 1As shown in (a), this action can be modeled using two key points: before tilting the container, as shown in (a). Figure 1 As shown in (b), and after tilting the container, as Figure 1 As shown in (c). Figure 1 In the diagram, A represents the workpiece coordinate system, a Cartesian coordinate system based on the workpiece. B represents the tool coordinate system, a Cartesian coordinate system based on the robot's end effector; here, it's equivalent to the coordinate system based on the water cup held by the robot. The trajectory between key points can be determined using traditional trajectory planning methods to find the optimal trajectory, without the need for additional modeling.

[0126] This invention assumes that the trajectory of a specific task is a sequence traj of trajectory points obtained by uniform sampling, where

[0127] traj={point1, point2,…,point i ...point n} (2.1)

[0128] point i =[x i y i , z i a i b i c i d i ] T (2.2)

[0129] Where, point i x represents the information of the i-th trajectory point. i y i , z i Let a represent the Cartesian coordinates of key point i in the workspace. i b i c i d i This represents the quaternion of keypoint i in the workspace.

[0130] Keypoints kp∈traj are trajectory points sampled from the sequence that possess salient features characterizing the task. The sequence of k keypoints is used to model the task's actions.

[0131] action = kp1, kp2, ..., kp j ..., kp k (2.3)

[0132] kp j =[x j y j , z ja j b j c j d j ] T (2.4)

[0133] kp j This represents the j-th trajectory point sampled from the sequence traj that possesses characteristics of the task.

[0134] Here, an object coordinate system is used as the reference frame for the workspace. The object coordinate system is defined with reference to the target object, and its origin and three axes are defined. Figure 2 This demonstrates the object coordinate system definition when the target object is an orange container during a liquid-pouring action. The origin of the world coordinate system is defined at the corner of the table, as shown by the blue coordinate axis in the figure. w Y w Z w These represent the x, y, and z axes of the world coordinate system, respectively; the origin of the object coordinate system is defined at the centroid of the target container, as shown by the black coordinate axes in the figure. o Y o Z o These represent the x, y, and z axes of the object's coordinate system, respectively.

[0135] The advantages of keypoint-based robot motion modeling methods lie primarily in their abstraction and generalization. First, modeling motion using all trajectory points leads to information redundancy, as some points can be obtained from others through interpolation. Second, the idea behind keypoint-based modeling is to abstract all trajectory points, retaining key information while eliminating noise and non-key information, resulting in better generalization. Furthermore, since modeling is done with the object's coordinate system as a reference, key points reflect the relative positional relationship between the robot arm's end effector and the target object, making keypoint-based modeling methods more robust to targets in different positions.

[0136] Based on keypoint-based robot motion modeling, this invention defines the robot motion planning problem for customized tasks as a prediction problem of k keypoints given an initial state. These k keypoints need to meet two requirements: first, the set of keypoints must be able to represent the action pattern of the task, i.e., possess certain action characteristics; second, the set of keypoints must be able to adapt to different initial states.

[0137] (2) Trajectory segmentation method based on first-order difference metric

[0138] The previous section modeled robot motion as k keypoints. To learn features from example trajectories for predicting keypoints, we must first extract these keypoints. This section presents a trajectory segmentation method based on a first-order difference metric for extracting keypoints from example trajectories.

[0139] First, define the first-order difference component of the trajectory data. Given trajectory data...

[0140] traj={initpoint, point1,..., point n} (2.5)

[0141] Under the given conditions, the first-order difference component is defined as the difference between adjacent trajectory points.

[0142] Δtraj=point1-initpoint, point2-point1,…,point i -point i-1 ...point n -point n-1 (2.6)

[0143] in

[0144]

[0145] To extract key points from the trajectory, this invention makes the following assumptions: a uniformly sampled trajectory can be divided into k segments based on the velocity characteristics of the trajectory points, with each segment having similar velocity magnitude and direction. After obtaining the segmentation results, the segmentation points and the endpoint of the trajectory are used as key points.

[0146] In the trajectory sequence obtained through uniform discrete sampling, the first-order difference component can represent the velocity vector, reflecting the magnitude and direction of the velocity during the robot's end effector motion. Based on the first-order difference component, a trajectory can be segmented, and the loss function after segmentation is defined as the sum of the variances of the first-order difference components of each trajectory segment. Based on the above analysis, this invention designs a trajectory segmentation method based on the first-order difference metric.

[0147] This invention borrows the idea of ​​minimizing the variance of each class cluster in the k-means method to design a trajectory segmentation method based on the first-order difference metric. The algorithm is described below. This invention assumes that a trajectory consists of k parts, and that these k parts can be distinguished by some feature. Since the first-order difference component has the property of velocity, including both magnitude and direction, segmenting the trajectory into different parts based on velocity magnitude and direction aligns with human intuitive understanding of trajectories; therefore, the first-order difference component is chosen as the feature. Based on the sequential nature of trajectory data, this method defines k segmentation points, which are then traversed along the time axis to find the k optimal segmentation points through a traversal approach. This minimizes the sum of the variances of the first-order difference components of the data in each trajectory segment. The pseudocode of the algorithm is as follows.

[0148]

[0149]

[0150] Here, the value of k represents the assumption made about the number of segments in the trajectory. When the value of k is too small, it is easy to fail to fit the trajectory, i.e., underfitting. In this case, it may not be able to completely reproduce the true trajectory. When the value of k is too large, it is easy to overfit the trajectory, i.e., overfitting. In this case, it may fit many noise points, resulting in poor generalization of the reproduced trajectory. In summary, a reasonable value of k is an important prior information for trajectory segmentation tasks.

[0151] Taking the task of pouring liquid as an example, the trajectory can be divided into two parts, i.e., k=2. The first part (initpoint, point) m The first part is moving to the bottle opening; this part mainly involves translation. The second part (point) m+1 ,point n The process of tilting is the main part, which involves rotation. Therefore, the key points extracted from the trajectory of this task are the points. m and point n .

[0152] (3) Key point prediction method based on PCA and quadratic programming

[0153] This section uses PCA for keypoint feature extraction, first defining the concept of a keypoint vector. In the keypoint prediction problem, the prediction result should be the three-dimensional spatial coordinates and quaternions of k keypoints. This invention defines the prediction result as a 7*k dimensional vector, i.e., the keypoint vector vec = [x1, y1, z1, a1, b1, c1, d1, ..., x k y k , z k a kb k c k d k ] T .

[0154] This invention assumes that action patterns can be characterized by two attributes of keypoint vectors: the correlation between the dimensions of the keypoint vectors and the value range of each dimension of the keypoint vectors.

[0155] For example, in a two-dimensional plane, when the number of keypoints k=1, it can be achieved through... y = x 2 (2.8)

[0156] 0≤x≤1 (2.9)

[0157] It represents a movement pattern that conforms to a "quadratic curve" relationship. For example... Figure 3 As shown in the figure, the black scatter dots represent the distribution of keypoints extracted from the example data. The red rectangles in the figure represent the value range of each dimension of the keypoint vector, i.e.

[0158] 0≤x≤1 (2.10)

[0159] 0≤y≤1 (2.11)

[0160] After PCA transformation, it's equivalent to finding a new basis [x′, y′] to construct a coordinate system, mapping the original data to the new coordinate system. After the transformation, the range of values ​​for each dimension of the keypoint vector can be represented by the blue rectangle, i.e.

[0161]

[0162]

[0163] Furthermore, PCA reflects the primary linear correlations between the various dimensions of the data through the principal direction. In this example, the principal direction is...

[0164] y = x (2.14)

[0165] This indicates that the two dimensions of the data, x and y, have an approximate linear relationship of y = x.

[0166] In summary, PCA transformation can reflect the correlation between the various dimensions of the keypoint vector and obtain a more accurate range of values ​​for each dimension of the keypoint vector. These properties of PCA align with the assumption of this invention that "action patterns can be characterized by the correlation between the various dimensions of keypoints and their value ranges." Therefore, this invention employs PCA to extract action pattern features.

[0167] It is important to note that PCA is essentially a linear mapping based on maximizing variance, which can extract linear correlations between different dimensions of data. When there are non-linear correlations between different dimensions of keypoint data, PCA cannot accurately model such relationships. Therefore, this invention actually uses PCA to extract the linear correlations between various dimensions of keypoint vectors to approximate non-linear relationships.

[0168] Based on the pattern features extracted from key points using PCA, this invention transforms the key point prediction problem into a constrained quadratic programming problem.

[0169] The constraints of the optimization problem are determined by PCA, including linear inequality constraints and equality constraints. This section first derives the form of the constraints for the key point prediction problem, and then gives the expression of the optimization objective.

[0170] The solution space of the optimization problem is defined in a 7k-dimensional vector space, with the independent variable being a 7k-dimensional vector of key points.

[0171] X=[x1,y1,z1,a1,b1,c1,d1,…,x k y k , z k a k b k c k d k ] T (2.15)

[0172] PCA transformation is expressed as the formula

[0173] Y = A T X (2.16)

[0174] in,

[0175] Y = [y1, y2, ..., y] 7k ] T (2.17)

[0176] X and Y are both 7k-dimensional column vectors, A T The transformation matrix is ​​7k×7k. Linear constraints can be constructed based on the PCA transformation shown in formula (4.29).

[0177] Given a set of m data

[0178] X = [X1, X2, ..., X] m ] p×m (2.18)

[0179] p represents the dimension of each example trajectory (extracted key points) vector, p = 7 × k; m represents the number of example trajectories, where m trajectories (key points) are represented in vector form.

[0180] Data obtained after PCA transformation

[0181] Y = [Y1, Y2, ..., Y] m ] p×m (2.19)

[0182] Let Y be the element in the i′ row and j′ column of matrix Y. i′j′ Let be the value of the i′th dimension of the j′th data point.

[0183]

[0184] Therefore, for the dimensions corresponding to the q principal components of Y, the following inequality constraints are constructed.

[0185]

[0186]

[0187] A represents T The first q lines.

[0188] For the remaining pq-dimensional features of Y, since the variance after PCA is small, we have

[0189]

[0190] Therefore, for these pq-dimensional features, the following equality constraints are constructed.

[0191]

[0192] A represents T The last pq line, the right side of the equation represents the expectation of each dimension of the pq-dimensional non-principal components after PCA transformation, and constructs linear equality constraints.

[0193] This invention defines the optimization objective of the optimization problem as the sum of squares of the Euclidean distances between adjacent key points, expressed as formula (2.25).

[0194]

[0195] Where k represents the number of key points, [x i y i , z i ] T The Cartesian coordinates of the key points are represented by [x0, y0, z0]. TThis represents the Cartesian coordinates of the robot's end effector gripper in its initial state.

[0196] As we can see, the optimization objective is a quadratic function of the independent variable, which can be represented in matrix form.

[0197] tar=X T HX+gX+ε (2.26)

[0198] Where X represents the independent variable, H represents the quadratic term matrix, g represents the linear term matrix, and ε represents the constant term in the objective function. It is important to note that in optimization problems, the constant term in the objective function does not affect the optimization result; therefore, the constant term in tar is omitted.

[0199] When k=1

[0200] tar = (x1 - x0) 2 +(y1-y0) 2 +(z1-z0) 2 (2.27)

[0201] After omitting the constant term,

[0202] tar=x1 2 +y1 2 +z1 2 -2x0x1-2y0y1-2z0z1 (2.28)

[0203] Correspondingly,

[0204]

[0205] g=[-2x0 -2y0 -2z0 0 0 0 0] (2.30)

[0206] When k=2

[0207] tar = 2x1 2 +2y1 2 +2z1 2 +x2 2 +y2 2 +z2 2

[0208] 2x0x1-2y0y1-2z0z1-2x1x2-2y1y2-2z1z2 (2.31)

[0209] Correspondingly,

[0210]

[0211] g=[-2x0 -2y0 -2z0 0 0 0 0] (2.33)

[0212] As we can see, matrix H is an upper triangular matrix, and the diagonal elements only take the values ​​0, 1, and 2. Therefore, matrix H is a positive semi-definite matrix, and the objective function is a convex quadratic function.

[0213] In the optimization problem constructed in this invention, the constraints are linear equality and inequality constraints, and the objective function is a convex quadratic function. Thus, this invention models the robot motion planning problem as a keypoint prediction problem, and further transforms it into a constrained convex quadratic programming problem, making it solvable using mature convex quadratic programming methods.

[0214] This invention proposes a rapid motion planning system for robots, comprising: an example motion modeling module, a key point extraction module, and a motion prediction module.

[0215] Example Action Modeling Module: Models example actions using a sequence of trajectory points that can characterize task features;

[0216] Key point extraction module: The key points of the example trajectory are extracted using a trajectory segmentation method based on first-order difference metric;

[0217] Action prediction module: PCA is used to obtain the action pattern features of key points of example trajectory, the prediction constraints are determined, and the rapid action prediction under a given robot state is achieved by solving the quadratic programming problem under the constraints, thus completing the planning.

[0218] When the example action modeling module models example actions using a sequence of trajectory points that can characterize task features, it assumes that the example trajectory for a specific task is a sequence of trajectory point information traj obtained by uniform sampling, where...

[0219] traj={initpoint, point1, point2,…, pointi,…point n}

[0220] point i =[x i y i , z i a i b i c i d i ] T

[0221] Where initpoint represents the initial trajectory point information, point i x represents the information of the i-th trajectory point. i y i , z iLet a represent the Cartesian coordinates of the i-th trajectory point in the workspace. i b i c i d i Let represent the quaternion of the i-th trajectory point in the workspace.

[0222] The specific implementation method of the key point extraction module is as follows:

[0223] (1) Given example trajectory data traj = {initpoint, point1, ..., point2} containing n+1 pose points, the following conditions are met: n}, then the first-order difference component is defined as the difference between adjacent trajectory points, and the first-order difference component sequence Δtraj satisfies Δtraj=point1-initpoint, point2-point1, …, point i -point i-1 ...point n -point n-1

[0224] Wherein, the i-th first-order difference component satisfies

[0225]

[0226] (2) Based on the first-order difference component sequence, all the segmentation methods are traversed to find k optimal segmentation points on the example trajectory. The k optimal segmentation points divide the example trajectory data into k+1 segments, and the sum of the variances of the first-order difference components of each segment of trajectory data is minimized.

[0227] (3) The optimal segmentation point is the key point on the example trajectory.

[0228] The action prediction module uses PCA to obtain action pattern features of key points in the example trajectory, and the implementation method for determining the prediction constraints is as follows:

[0229] The solution space of the quadratic programming problem is defined in a 7×k-dimensional vector space, with the independent variable being the 7×k-dimensional key point vector X:

[0230] X=[x1,y1,z1,a1,b1,c1,d1,…,x k y k , z k a k b k c k d k ] T

[0231] PCA transformation is expressed as the formula

[0232] Y = A TX

[0233] in,

[0234] Y = [y1, y2, ..., y] 7k ] T

[0235] X and Y are both 7×k dimensional column vectors, A T The transformation matrix is ​​(7×k)×(7×k);

[0236] Given a set of m data

[0237] X = [X1, X2, ..., X] m ] p×m

[0238] p represents the dimension of the keypoint vector for each example trajectory, p = 7 × k; m represents the number of example trajectories;

[0239] Data obtained after PCA transformation

[0240] Y = [Y1, Y2, ..., Y] m ] p×m

[0241] Let Y be the element in the i′ row and j′ column of matrix Y. i′j′ Let be the value of the i′th dimension of the j′th data point.

[0242]

[0243] Therefore, for the dimensions corresponding to the q principal components of the Y matrix, the following inequality constraints are constructed.

[0244]

[0245]

[0246] A represents T The first q lines;

[0247] For the remaining pq dimensions of the Y matrix, since the variance after PCA is small, we have

[0248]

[0249] Therefore, for these pq-dimensional features, the following equality constraints are constructed.

[0250]

[0251] A represents TThe last pq row, the right side of the equation represents the expectation of each dimension of the pq-dimensional nonprincipal components after PCA transformation;

[0252] The inequality constraints and equality constraints mentioned above are the predictive constraints.

[0253] The action prediction module achieves rapid action prediction for a given robot state by solving a quadratic programming problem under constraints. The implementation method is as follows:

[0254] The optimization objective tar of the quadratic programming problem is defined as the sum of the squares of the Euclidean distances between adjacent key points, expressed as:

[0255]

[0256] Where k represents the total number of key points, [x i y i , z i ] T Let [x0, y0, z0] represent the Cartesian coordinates of the i-th key point. T This represents the Cartesian coordinates of the robot's end effector gripper in its initial state;

[0257] Therefore, the optimization objective tar is a quadratic function of the independent variable, represented in matrix form.

[0258] tar=X T HX+gX+ε

[0259] Where X represents the independent variable, H represents the quadratic term matrix, g represents the linear term matrix, and ε represents the constant term in the optimization objective;

[0260] Find the matrix H corresponding to different values ​​of k. Matrix H is an upper triangular matrix, and the diagonal elements only take the values ​​0, 1, and 2. Therefore, H is a positive semi-definite matrix. Thus, the optimization objective tar is a convex quadratic function. By using the mature convex quadratic programming method to solve it, the prediction of the robot's rapid actions under the given initial state can be obtained.

[0261] To improve the efficiency and flexibility of motion planning for robots performing customized tasks, this invention designs a motion planning method based on PCA and quadratic programming. The designed key point modeling method for robot motion can reasonably abstract the robot's motion trajectory. Combined with traditional planning methods such as interpolation, it can accurately represent specific motion trajectories with high precision and simplified, efficient modeling.

[0262] The trajectory segmentation method based on first-order difference metric can segment trajectories using their velocity attributes. It only requires setting the number of trajectory segments based on the action without any additional settings. Its accuracy and execution efficiency can meet the requirements of rapid adaptation to customized tasks in industrial scenarios.

[0263] A keypoint prediction method based on PCA and quadratic programming transforms robot motion planning into a constrained optimization problem. "Constraints" represent motion features, and "optimization" improves trajectory simplicity and execution speed. This method can predict keypoints based on motion features learned from teaching data, achieving high prediction accuracy and meeting the requirements of motion planning in industrial scenarios.

[0264] Although the present invention has been disclosed above with reference to preferred embodiments, it is not intended to limit the present invention. Any person skilled in the art can make possible changes and modifications to the technical solutions of the present invention by utilizing the methods and techniques disclosed above without departing from the spirit and scope of the present invention. Therefore, any simple modifications, equivalent changes and alterations made to the above embodiments based on the technical essence of the present invention without departing from the content of the technical solutions of the present invention shall fall within the protection scope of the technical solutions of the present invention.

Claims

1. A method for rapid motion planning of a robot, characterized in that, include: Model example actions using trajectory point sequences that can characterize task features; Key points of example trajectories are extracted using a trajectory segmentation method based on first-order difference metric. PCA is used to obtain the motion pattern features of key points of example trajectories, and prediction constraints are determined. By solving the quadratic programming problem under the constraints, fast motion prediction under a given robot state is achieved, thus completing the planning. Key points of the example trajectory are extracted using a trajectory segmentation method based on first-order difference metric. The specific implementation method is as follows: (1) Given example trajectory data traj = {initpoint, point1, ..., point2} containing n+1 pose points, the following conditions are met: n }, then the first-order difference component is defined as the difference between adjacent trajectory points, and the first-order difference component sequence Δtraj satisfies Δtraj=point1-initpoint,point2-point1,…,point i -point i-1 ...point n -point n-1 Wherein, the i-th first-order difference component satisfies x i ,y i ,z i Let a represent the Cartesian coordinates of the i-th trajectory point in the workspace. i ,b i ,c i ,d i Let represent the quaternion of the i-th trajectory point in the workspace; (2) Based on the first-order difference component sequence, all the segmentation methods are traversed to find k optimal segmentation points on the example trajectory. The k optimal segmentation points divide the example trajectory data into k+1 segments, and the sum of the variances of the first-order difference components of each segment of trajectory data is minimized. (3) The optimal segmentation point is the key point on the example trajectory.

2. The robot rapid motion planning method according to claim 1, characterized in that, When modeling example actions using a sequence of trajectory points that can characterize task features, it is assumed that the example trajectory of a certain task is a sequence traj of trajectory point information obtained by uniform sampling, where... traj={initpoint,point1,point2,…,point i ,…point n } point i =[x i ,y i ,z i ,a i ,b i ,c i ,d i ] T Where initpoint represents the initial trajectory point information, point i This represents the information of the i-th trajectory point.

3. The robot rapid motion planning method according to claim 1, characterized in that, The action pattern features of key points in the example trajectory are obtained using PCA, and the implementation method of the prediction constraints is determined as follows: The solution space of the quadratic programming problem is defined in a 7×k-dimensional vector space, with the independent variable being the 7×k-dimensional key point vector X: X=[x1,y1,z1,a1,b1,c1,d1,...,x k ,y k ,z k ,a k ,b k ,c k ,d k ] T PCA transformation is expressed as the formula Y=A T X in, Y=[y1,y2,...,y 7k ] T X and Y are both 7×k dimensional column vectors, A T The transformation matrix is ​​(7×k)×(7×k); Given a set of m data X=[X1,X2,...,X m ] p×m p represents the dimension of the keypoint vector for each example trajectory, p = 7 × k; m represents the number of example trajectories; Data obtained after PCA transformation Y=[Y1,Y2,..,Y m ] p×m Let Y be the element in the i′ row and j′ column of matrix Y. i′j′ Let be the value of the i′th dimension of the j′th data point. Therefore, for the dimensions corresponding to the q principal components of the Y matrix, the following inequality constraints are constructed. A represents T The first q lines; For the remaining pq dimensions of the Y matrix, since the variance after PCA is small, we have Therefore, for these pq-dimensional features, the following equality constraints are constructed. A represents T The last pq row, the right side of the equation represents the expectation of each dimension of the pq-dimensional nonprincipal components after PCA transformation; The inequality constraints and equality constraints mentioned above are the predictive constraints.

4. The robot rapid motion planning method according to claim 3, characterized in that, Rapid motion prediction for a given robot state is achieved by solving a quadratic programming problem under constraints, as follows: The optimization objective tar of the quadratic programming problem is defined as the sum of the squares of the Euclidean distances between adjacent key points, expressed as: Where k represents the total number of key points, [x i y i , z i ] T Let [x0, y0, z0] represent the Cartesian coordinates of the i-th key point. T This represents the Cartesian coordinates of the robot's end effector gripper in its initial state; Therefore, the optimization objective tar is a quadratic function of the independent variable, represented in matrix form. tar=X T HX+gX+e Where X represents the independent variable, H represents the quadratic term matrix, g represents the linear term matrix, and ε represents the constant term in the optimization objective; Find the matrix H corresponding to different values ​​of k. Matrix H is an upper triangular matrix, and the diagonal elements only take the three values ​​0, 1, and 2. Therefore, H is a positive semi-definite matrix. Thus, the optimization objective tar is a convex quadratic function. By using the mature convex quadratic programming method, the prediction of the robot's rapid actions under a given initial state can be obtained.

5. A rapid motion planning system for a robot, characterized in that, include: Example of a motion modeling module, key point extraction module, and motion prediction module; Example Action Modeling Module: Models example actions using a sequence of trajectory points that can characterize task features; Key point extraction module: The key points of the example trajectory are extracted using a trajectory segmentation method based on first-order difference metric; Action prediction module: PCA is used to obtain the action pattern features of key points of example trajectory, the prediction constraints are determined, and the rapid action prediction under a given robot state is achieved by solving the quadratic programming problem under the constraints, thus completing the planning. The specific implementation method of the key point extraction module is as follows: (1) Given example trajectory data traj = {initpoint, point1, ..., point2} containing n+1 pose points, the following conditions are met: n }, then the first-order difference component is defined as the difference between adjacent trajectory points, and the first-order difference component sequence Δtraj satisfies Δtraj=point1-initpoint,point2-point1,…,point i -point i-1 ,…point n -point n-1 Wherein, the i-th first-order difference component satisfies x i ,y i ,z i Let a represent the Cartesian coordinates of the i-th trajectory point in the workspace. i ,b i ,c i ,d i Let represent the quaternion of the i-th trajectory point in the workspace; (2) Based on the first-order difference component sequence, all the segmentation methods are traversed to find k optimal segmentation points on the example trajectory. The k optimal segmentation points divide the example trajectory data into k+1 segments, and the sum of the variances of the first-order difference components of each segment of trajectory data is minimized. (3) The optimal segmentation point is the key point on the example trajectory.

6. A robot rapid motion planning system according to claim 5, characterized in that, When the example action modeling module models example actions using a sequence of trajectory points that can characterize task features, it assumes that the example trajectory for a certain task is a sequence of trajectory point information traj obtained by uniform sampling, where... traj={initpoint,point1,point2,…,point i ,…point n } point i =[x i ,y i ,z i ,a i ,b i ,c i ,d i ] T Where initpoint represents the initial trajectory point information, point i This represents the information of the i-th trajectory point.

7. A robot rapid motion planning system according to claim 5, characterized in that, The action prediction module uses PCA to obtain action pattern features of key points in the example trajectory, and the implementation method for determining the prediction constraints is as follows: The solution space of the quadratic programming problem is defined in a 7×k-dimensional vector space, with the independent variable being the 7×k-dimensional key point vector X: X=[x1,y1,z1,a1,b1,c1,d1,...,x k ,y k ,z k ,a k ,b k ,c k ,d k ] T PCA transformation is expressed as the formula Y=A T X in, Y=[y1,y2,...,y 7k ] T X and Y are both 7×k dimensional column vectors, A T The transformation matrix is ​​(7×k)×(7×k); Given a set of m data X=[X1,X2,...,X m ] p×m p represents the dimension of the keypoint vector for each example trajectory, p = 7 × k; m represents the number of example trajectories; Data obtained after PCA transformation Y=[Y1,Y2,...,Y m ] p×m Let Y be the element in the i′ row and j′ column of matrix Y. i′j′ Let be the value of the i′th dimension of the j′th data point. Therefore, for the dimensions corresponding to the q principal components of the Y matrix, the following inequality constraints are constructed. A represents T The first q lines; For the remaining pq dimensions of the Y matrix, since the variance after PCA is small, we have Therefore, for these pq-dimensional features, the following equality constraints are constructed. A represents T The last pq row, the right side of the equation represents the expectation of each dimension of the pq-dimensional nonprincipal components after PCA transformation; The inequality constraints and equality constraints mentioned above are the predictive constraints.

8. A robot rapid motion planning system according to claim 5, characterized in that, The action prediction module achieves rapid action prediction for a given robot state by solving a quadratic programming problem under constraints. The implementation method is as follows: The optimization objective tar of the quadratic programming problem is defined as the sum of the squares of the Euclidean distances between adjacent key points, expressed as: Where k represents the total number of key points, [x i y i , z i ] T Let [x0, y0, z0] represent the Cartesian coordinates of the i-th key point. T This represents the Cartesian coordinates of the robot's end effector gripper in its initial state; Therefore, the optimization objective tar is a quadratic function of the independent variable, represented in matrix form. tar=X T HX+gX+e Where X represents the independent variable, H represents the quadratic term matrix, g represents the linear term matrix, and ε represents the constant term in the optimization objective; Find the matrix H corresponding to different values ​​of k. Matrix H is an upper triangular matrix, and the diagonal elements only take the three values ​​0, 1, and 2. Therefore, H is a positive semi-definite matrix. Thus, the optimization objective tar is a convex quadratic function. By using the mature convex quadratic programming method, the prediction of the robot's rapid actions under a given initial state can be obtained.

Images