Path planning method and system based on Gaussian mixture model and model predictive control

Abstract

The present invention provides a path planning method and system based on a Gaussian mixture model and model predictive control, the method comprising: first constructing a position set of key points of human hand motion in continuous time; then constructing a joint variable and an observation data set based on the joint variable, wherein the joint variable consists of time and the spatial position of the human hand at the corresponding moment; constructing a Gaussian mixture model and using the observation data set to train the model and determine the model parameters; finally, introducing model predictive control, i.e., establishing a state space model of the robotic arm, then constructing an objective function based on the state space model and introducing the Gaussian mixture model's prediction result of the human hand trajectory into the constraint condition, thereby realizing the optimal path planning of the robotic arm from the current position to the reference trajectory. Through the above technical solution, the robotic arm can effectively adapt to changes in the human hand's motion trajectory while avoiding the phenomenon of incoordination or collision during the execution of the task, thereby greatly improving the efficiency and safety of human-machine collaboration.

Description

Technical Field

[0001] The present invention belongs to the field of robotics technology, and in particular relates to a path planning method and system based on a Gaussian mixture model and model predictive control. Background Art

[0002] With the rapid development of robotics and artificial intelligence technologies, precise control and path planning of robotic arms have become a key research area in the field of automation. In particular, in areas such as service robotics, medical rehabilitation, industrial manufacturing, and human-machine interaction, robotic arms must not only complete predetermined tasks but also perceive and respond to human dynamic movements in real time to ensure efficient and safe completion of complex tasks. These tasks often require robotic arms to possess the capabilities of rapid adaptation, precise control, and efficient planning when working alongside humans, thereby improving operational efficiency, reducing human-machine conflicts, and ensuring operational safety.

[0003] Traditional robotic arm path planning methods primarily rely on pre-set trajectories or fixed control algorithms. These methods are typically based on static planning models and struggle to effectively adapt to dynamically changing environments. They present significant limitations, particularly in human-machine interaction and multi-objective tasks. For example, when working with a human hand, traditional methods struggle to adapt to the randomness and uncertainty of human motion. This results in the robotic arm being unable to effectively predict and respond to changes in the hand's motion trajectory, making it impossible to plan a reasonable, collision-free path, which in turn impacts the performance and safety of the task.

[0004] To address these issues, machine learning-based prediction and control methods have become a research hotspot in path planning in recent years. In particular, the Gaussian mixture model (GMM), as an effective probabilistic model, demonstrates unique advantages in handling data with uncertainty and noise. By modeling a combination of multiple Gaussian distributions, GMMs can capture the underlying patterns in complex data and are widely used in predicting and modeling hand trajectories. By learning from the distribution patterns in historical data, GMMs can accurately predict the future position of a hand, making them particularly suitable for capturing dynamic changes and uncertainties in real time.

[0005] The advantage of the Gaussian mixture model lies in its non-parametric nature, allowing it to adaptively adjust its complexity based on varying data distributions. This gives it strong generalization capabilities when working with real-time data. In hand trajectory prediction, GMMs can model historical data to capture the temporal characteristics of hand movements and infer future trajectories based on this information. Due to its excellent ability to handle data uncertainty and noise, GMMs can effectively predict hand position changes in complex environments, providing accurate data support for subsequent path planning.

[0006] During path planning, Model Predictive Control (MPC), an advanced control method, can adapt to changes in dynamic environments by optimizing real-time solutions to problems. MPC uses a rolling optimization strategy for path planning based on model predictions, predicting future actions at every moment and adjusting decisions in real time based on environmental changes. Within the MPC framework, using the hand position predicted by the Generative Model (GMM) as a constraint effectively incorporates predicted information about the human hand's trajectory into the robot's path planning, optimizing the robot's trajectory and ensuring flexible path adjustment during interaction with humans, avoiding collisions and ensuring successful task completion.

[0007] While research has explored the combined application of Gaussian mixture models and model predictive control (MPC), accurately using the complex hand trajectories predicted by GMM as constraints for MPC and performing real-time path optimization in complex, high-dimensional environments remains a challenging problem. Existing technologies often face challenges such as high computational complexity, poor real-time performance, and model instability when addressing optimization problems in high-dimensional, dynamic environments. Therefore, efficiently integrating these two technologies and applying them in practical systems remains a pressing technical challenge. Summary of the Invention

[0008] In response to the technical integration problem of the above-mentioned MPC and GMM, the technical solution of the present invention proposes a new method that combines the Gaussian mixture model GMM with the model predictive control MPC, namely, a path planning method and system based on the Gaussian mixture model and model predictive control, wherein the hand position predicted by the GMM is used as the constraint item of the MPC, thereby realizing real-time optimization of the path planning of the robot arm. This method can not only significantly improve the tracking accuracy of the robot arm for the trajectory of the human hand, but also ensure that the path planning of the robot arm in a dynamic environment has better flexibility and naturalness through real-time optimization control. Through this method, the robot arm can effectively adapt to the changes in the trajectory of the human hand, while avoiding the phenomenon of incoordination or collision during the execution of the task, thereby greatly improving the efficiency and safety of human-machine collaboration.

[0009] To this end, the present invention provides the following technical solutions:

[0010] On the one hand, the technical solution of the present invention provides a path planning method based on a Gaussian mixture model and model predictive control, comprising the following steps:

[0011] S1: Collect human motion data in a continuous time series to obtain the position set of key points of human hand motion as the human hand spatial position set to represent the human hand trajectory;

[0012] S2: Construct joint variables and observation data sets based on joint variables, where the joint variables are composed of time t and the spatial position of the hand at the corresponding moment composition;

[0013] S3: Construct a Gaussian mixture model, that is, the distribution of the joint variable is represented by the weighted sum of multiple Gaussian distributions;

[0014] S4: Using the observation data set to train the Gaussian mixture model to determine the model parameters of the Gaussian mixture model , are the weight, mean vector and covariance matrix of the k-th Gaussian distribution respectively;

[0015] S5: Introducing model predictive control, i.e., establishing a state space model of the robotic arm, constructing an objective function based on the state space model, and introducing the hand trajectory prediction results of the Gaussian mixture model into the constraint conditions to achieve optimal path planning between the robotic arm's current position and the reference trajectory;

[0016] The Gaussian mixture model is used to predict the trajectory of the human hand of the robotic arm at a future moment, and then a collision avoidance constraint term is constructed based on the position state of the end of the robotic arm at the same moment obtained based on the state space model.

[0017] Further preferably, the collision avoidance constraint item is expressed as:

[0018] ;

[0019] in, It indicates the position state of the end of the manipulator in the next k steps based on the state space model, i.e., the position. Indicates the use of Gaussian mixture model to predict the hand trajectory in the next k steps, that is, the position of the key points of the hand movement, t is the current time, H is the number of time steps predicted from the current time t to the future, Indicates the safety distance threshold, represents any time step within the forecast horizon.

[0020] Further preferably, the objective function is expressed as:

[0021] ;

[0022] in, represents the objective function, represents the target state of the end of the robotic arm in the next k steps, and Both represent weight coefficients, is the input control quantity of the robot controller in the next k steps.

[0023] Further preferably, the constraint conditions of the predictive control model also include a combination of one or more of input control quantity constraints, robot arm angle and angular velocity constraints, and robot arm kinematic constraints.

[0024] Further preferably, the process of using the Gaussian mixture model to predict the trajectory of the human hand of the robotic arm at a future moment is as follows:

[0025] For the prediction time t, the mean and covariance of the trained Gaussian mixture model parameters are first decomposed:

[0026] ;

[0027] in, represents the mean vector of the k-th Gaussian distribution, represents the mean of the k-th Gaussian distribution with respect to the time variable t, represents the mean value of the k-th Gaussian distribution about position P, represents the covariance matrix of the k-th Gaussian distribution, represents the variance of the time variable t, represents the covariance between time t and position P, represents the covariance of position P, yes The transposed matrix of

[0028] Then calculate the posterior of each Gaussian regression model at time t:

[0029] ;

[0030] in, represents the posterior weight of the k-th Gaussian distribution at a given time t, The mean is , the covariance is The Gaussian probability density function of The mean is , the covariance is The Gaussian probability density function of For the j The weight corresponding to the Gaussian distribution;

[0031] Then, calculate the conditional mean and covariance of each Gaussian distribution:

[0032] ;

[0033] in, represents the output mean prediction of the k-th Gaussian distribution at time t, represents the output covariance prediction of the kth Gaussian component at time t;

[0034] Finally, calculate the result of the weighted average prediction:

[0035] ;

[0036] in, It represents the expected value of the predicted output at a given time t, that is, the hand trajectory at the predicted time t, and K is the number of Gaussian distributions.

[0037] Further preferably, in step S4, the process of training the Gaussian mixture model using the observation data set is to introduce the expectation maximization algorithm EM to learn the parameters of the Gaussian mixture model. , as follows:

[0038] First randomly initialize the parameters to obtain the parameters after initialization , that is, k The initial values ​​of the weights, mean vectors, and covariance matrices of the Gaussian distributions;

[0039] Then use the expectation maximization algorithm EM to iterate until the iteration termination condition is met to obtain the parameters of the final Gaussian mixture model, where step E: calculate each sample data point The posterior probability of belonging to the k-th Gaussian distribution; Step M: Update the parameters of the Gaussian mixture model using the posterior probability;

[0040] Among them, the posterior probability formula is:

[0041] ;

[0042] in, Indicates the nth sample data point in the i-th iteration The posterior probability from the kth Gaussian component; 、 They represent the kth Gaussian distribution, the kth Gaussian distribution and the kth Gaussian distribution in the i-th iteration respectively. j The weights of the Gaussian distribution; Indicates the k-th Gaussian distribution for the sample data point in the i-th iteration The probability density of Indicates the j-th Gaussian distribution for the sample data point in the i-th iteration The probability density of and Respectively represent the mean vectors of the kth and jth Gaussian distributions in the i-th iteration, and Respectively represent the covariance matrices of the kth and jth Gaussian distributions in the i-th iteration, K represents the total number of Gaussian distributions in the Gaussian mixture model, It represents the nth sample data point, that is, the location parameter of the joint variable at a moment in the observation data set is taken as a sample data point, and T is the matrix transpose symbol.

[0043] The parameter update formula of the Gaussian mixture model in the M step is as follows:

[0044] ;

[0045] ;

[0046] ;

[0047] ;

[0048] in, represents the number of valid samples of the kth Gaussian distribution in the i+1th iteration, and N represents the number of samples; The weight of the k-th Gaussian distribution at the i+1-th iteration; represents the mean vector of the k-th Gaussian distribution in the i+1-th iteration; represents the covariance matrix of the k-th Gaussian distribution in the i+1-th iteration.

[0049] Further preferably, the Gaussian mixture model constructed in step S3 is expressed as:

[0050] ;

[0051] Where K is the number of Gaussian distributions in the Gaussian mixture model, and the weight Satisfy the normalization conditions:

[0052] ;

[0053] The mean is And the covariance matrix is The probability density function of , exists:

[0054] ;

[0055] Where d is the joint variable The number of dimensions, T is the matrix transpose.

[0056] In another aspect, the technical solution of the present invention further provides a system based on the above-mentioned path planning method, comprising:

[0057] The human hand motion key point data acquisition module is used to collect human body motion data in a continuous time series and obtain the position set of human hand motion key points;

[0058] The joint variable construction module is used to construct joint variables and observation data sets based on joint variables. The joint variables are composed of time t and the spatial position of the hand at the corresponding moment. composition;

[0059] Gaussian mixture model construction module, used to construct a Gaussian mixture model, that is, the distribution of joint variables is represented by the weighted sum of multiple Gaussian distributions;

[0060] A training module is used to train the Gaussian mixture model using the observation data set to determine the model parameters of the Gaussian mixture model , are the weight, mean vector and covariance matrix of the k-th Gaussian distribution respectively;

[0061] A path planning module is used to introduce model predictive control, that is, to establish a state-space model of the robotic arm, then construct an objective function based on the state-space model, and introduce the Gaussian mixture model's prediction of the human hand trajectory into the constraints to achieve optimal path planning between the robotic arm's current position and the reference trajectory;

[0062] The Gaussian mixture model is used to predict the trajectory of the human hand of the robotic arm at a future moment, and then a collision avoidance constraint term is constructed based on the position state of the end of the robotic arm at the same moment obtained based on the state space model.

[0063] In a third aspect, the technical solution of the present invention further provides a computer device, comprising: one or more processors; and a memory storing a computer program; wherein the processor calls the computer program to implement:

[0064] A step of a path planning method based on Gaussian mixture model and model predictive control.

[0065] In a fourth aspect, the technical solution of the present invention further provides a computer-readable storage medium storing a computer program, wherein the computer program is called by a processor to implement:

[0066] A step of a path planning method based on Gaussian mixture model and model predictive control.

[0067] Beneficial effects:

[0068] The technical solution of the present invention uses a Gaussian mixture model to predict the motion trajectory of the human body. The Gaussian mixture model can effectively process dynamic data with noise and uncertainty, and use historical motion data to predict future motion trajectories. Compared with traditional methods, the Gaussian mixture model does not need to assume the specific distribution form of the data, and can flexibly respond to complex and nonlinear motion trajectory changes. Through prediction, the future position of the human hand can be accurately foreseen, thereby providing reliable prediction information for the motion planning of the robotic arm. To this end, the technical solution of the present invention targets the uncertainty modeling of the system state distribution, introduces a Gaussian mixture model for the probabilistic prediction of future states, embeds the GMM prediction results into the MPC framework as a constraint item, and guides the robotic arm to perform collision-free path planning, thereby more accurately characterizing the uncertainty characteristics of multi-peak / multi-modal than the traditional single distribution model.

[0069] The technical solution of the present invention predicts the motion trajectory of the human hand based on the Gaussian mixture model, further uses MPC to optimize and plan the path of the robotic arm, and introduces a weighted strategy in the objective function of the MPC, so that the objective function can adapt to different task scenarios, thereby improving the adaptability of the algorithm. In the preferred model predictive control, the optimal path of the robotic arm is calculated in real time by considering timing information, spatial constraints and motion models. During the path planning process, the system will use the motion trajectory of the human hand as a key constraint to ensure that the robotic arm can flexibly avoid obstacles and maintain appropriate coordination and coordination with the motion trajectory of the human hand. In addition, the dynamic time domain planning of the model predictive control can adjust the path according to real-time changes in the environment to avoid collisions and ensure the safety and efficiency of the robotic arm when performing tasks. BRIEF DESCRIPTION OF THE DRAWINGS

[0070] Figure 1 It is a flow chart of a path planning method based on a Gaussian mixture model and model predictive control provided by an embodiment of the present invention.

[0071] Figure 2 It is a schematic diagram of the architecture of electronic components of the electronic terminal provided by the present invention. DETAILED DESCRIPTION

[0072] In order to make the purpose, technical solutions and advantages of this application more clearly understood, the present application is further described in detail below with reference to the accompanying drawings and examples. It should be understood that the specific embodiments described herein are only used to illustrate the present invention and are not intended to limit the present invention. The technical features involved in the various embodiments of the present invention described below may be combined with each other as long as they do not conflict with each other.

[0073] It should be noted that although the device schematics illustrate functional module divisions and the flowcharts illustrate logical sequences, in certain circumstances, the steps shown or described may be performed in a sequence that differs from the module divisions in the device or the sequence in the flowcharts. The terms "first," "second," and so on, in the specification, claims, and drawings, are used to distinguish similar items and are not necessarily used to describe a specific sequence or precedence.

[0074] Unless otherwise defined, all technical and scientific terms used herein have the same meaning as those commonly understood by those skilled in the art to which this application pertains. The terms used herein are for the purpose of describing the embodiments of this application only and are not intended to limit this application.

[0075] The technical solution of the present invention constructs a Gaussian mixture model, uses the Gaussian mixture model to accurately predict the trajectory of the human hand, and uses it as an effective constraint in path planning. By combining model predictive control, efficient and collision-free path planning can be achieved in high-dimensional complex environments, and the robotic arm can be naturally and flexibly adapted to the movement trajectory of the human hand, ensuring that the robotic arm can efficiently avoid obstacles and other uncertain factors when performing tasks, thereby improving the safety and efficiency of task execution.

[0076] To this end, the technical solution of the present invention provides the following technical ideas:

[0077] S1: Collect human motion data in a continuous time series to obtain the position set of key points of human hand motion;

[0078] S2: Construct joint variables and observation datasets based on joint variables. The joint variables are composed of time t and the spatial position of the hand at the corresponding moment. composition;

[0079] S3: Construct a Gaussian mixture model, that is, the distribution of the joint variable is represented by the weighted sum of multiple Gaussian distributions;

[0080] S4: Use the observation data set to train the Gaussian mixture model and determine the model parameters of the Gaussian mixture model , are the weight, mean vector and covariance matrix of the k-th Gaussian distribution respectively;

[0081] S5: Introduce model predictive control, that is, establish a state-space model of the robotic arm, then construct an objective function based on the state-space model, and introduce the Gaussian mixture model's prediction results of the human hand trajectory into the constraint conditions to achieve optimal path planning between the robotic arm's current position and the reference trajectory; among them, use the Gaussian mixture model to predict the human hand trajectory of the robotic arm at future moments, and then construct a collision-free constraint item with the robotic arm's end position state at the same moment obtained based on the state-space model.

[0082] The present invention will be described in detail below with reference to specific embodiments.

[0083] Example 1:

[0084] An embodiment of the present invention provides a path planning method based on a Gaussian mixture model and model predictive control, comprising the following steps:

[0085] Step 1: Use a depth camera to obtain the 3D spatial position of the human body.

[0086] In this embodiment, a depth camera acquires the three-dimensional spatial position of a moving human body. The depth camera is mounted using an eye-out-of-hand configuration, and the robotic arm uses this eye-out-hand calibration method to determine the human's position. For example, a depth camera (such as Intel RealSense or Azure Kinect) is fixed in a static position outside the robotic arm's workspace, ensuring that its field of view covers both the human's motion area and the robotic arm's operating range. The camera must be able to simultaneously and stably capture key points on the human body and the robotic arm's end effector. This eye-out-hand calibration method is currently available and will not be described in detail.

[0087] The depth camera obtains the distance between the object and the camera by emitting infrared light and receiving reflected light, thereby generating depth information for each pixel and then calculating the three-dimensional coordinates based on the depth information. In this way, the depth camera can capture the coordinates of the human body in three-dimensional space. When the image obtained by the depth camera is ,in and is the pixel position in the image, Indicates the depth value of the pixel. The three-dimensional coordinates in the camera coordinate system It can be calculated using the following formula:

[0088] ;

[0089] ;

[0090] ;

[0091] in, is the coordinate of the human body in three-dimensional space, are the focal length of the camera, is the pixel coordinate of the center of the image.

[0092] Step 2: Use the MediaPipe framework to extract human motion: After using the depth camera to obtain image data, use the MediaPipe framework to extract key points of human hand motion , the key points in this embodiment Located at the wrist, if mediapipe automatically identifies the wrist key points, are the coordinate values ​​of the key points in three-dimensional space. By extracting the hand motion data in a continuous time series in real time, a set of key point positions of the hand motion can be obtained. Assume that there are N time sampling points in this set of time series, and the key points The coordinates at time t are defined as:

[0093] ;

[0094] Among them, t represents the time index, i represents the i-th key point, For key points At the coordinate of time t, the set of key points of hand movement is the key points collected in continuous time N positions A collection of locations.

[0095] The technical solution of the present invention uses the MediaPipe framework to extract the hand trajectory, and then inputs the hand movement trajectory into a Gaussian mixture model for hand movement prediction.

[0096] Step 3: Construction of joint variables: Construct a joint variable , which is a variable composed of time t and the spatial position of the hand at the corresponding moment Together, they are used to describe the spatiotemporal characteristics of hand movements. All the joint variables obtained from time t=0 to the end time T Record , which is expressed as:

[0097] ;

[0098] Where t represents the time index, Indicates the end time. represents the spatial coordinates of the hand, Represents a joint variable, which mainly stores the time of hand movement and the spatial position information of the hand, and is regarded as a trajectory data point.

[0099] Step 4: Build a Gaussian mixture model : Union variable In accordance with the Gaussian distribution, the position information of the joint variable at a moment is a sample data point, that is, the joint variable It can be expressed as a weighted sum of multiple Gaussian distributions:

[0100] ;

[0101] Where K is the number of Gaussian distributions, is the weight of the k-th Gaussian distribution, satisfying the normalization condition:

[0102] ;

[0103] The mean is , the covariance matrix is The probability density function of , exists:

[0104] ;

[0105] Where d is the joint variable The number of dimensions, T is the matrix transpose.

[0106] Step 5: Train the parameters of the Gaussian mixture model: Use the expectation maximization algorithm (EM algorithm) to learn the parameters in the Gaussian mixture model , where the parameters are randomly initialized first to obtain the parameters after initialization , and then use the expectation maximization algorithm to iterate, and calculate each data point in E steps The posterior probability of belonging to the k-th Gaussian distribution is:

[0107] ;

[0108] in, Indicates that in the i-th iteration, the n-th sample The posterior probability from the kth Gaussian component; represents the weight of the kth Gaussian distribution in the i-th iteration; Indicates that in the i-th iteration, the k-th Gaussian distribution is proportional to the sample The probability density of Indicates that in the i-th iteration, the j-th Gaussian distribution is proportional to the sample The probability density of and Respectively represent the mean vectors of the kth and jth Gaussian distributions in the i-th iteration, and Respectively represent the covariance matrices of the kth and jth Gaussian distributions in the i-th iteration, K represents the total number of Gaussian distributions in the Gaussian mixture model, Represents the nth sample data point.

[0109] In step M, the parameters of the hybrid model are re-estimated based on the responsibility value:

[0110] 1. Update the number of valid samples for each Gaussian distribution:

[0111] ;

[0112] in, It represents the number of valid samples of the kth Gaussian distribution in the i+1th iteration, and N represents the number of samples.

[0113] 2. Update component weights:

[0114] ;

[0115] in, The weight of the k-th Gaussian distribution at iteration (i+1).

[0116] 3. Update the mean:

[0117] ;

[0118] in, Represents the mean vector of the k-th Gaussian distribution in the i+1-th iteration.

[0119] 4. Update the covariance matrix:

[0120] ;

[0121] in, represents the covariance matrix of the k-th Gaussian distribution in the i+1-th iteration.

[0122] After the EM algorithm calculates the maximum expected value, it calculates the log-likelihood value of the current iteration:

[0123] ;

[0124] in, Represents the entire observation data set The log-likelihood value of N is the observed data set The number of samples.

[0125] When the log-likelihood of two consecutive iterations The change is less than the set threshold , or the algorithm terminates when the maximum number of iterations is reached, and the parameters of the optimal Gaussian regression model are obtained through the EM algorithm:

[0126] ;

[0127] in, Represents the set of all parameters of the GMM model.

[0128] It should be understood that after training the Gaussian regression model, the process of predicting the position P at a given time t is as follows:

[0129] Decompose the mean and covariance in the trained Gaussian mixture model parameters into:

[0130] ;

[0131] in, represents the joint mean vector of the k-th Gaussian component, represents the mean of the k-th Gaussian distribution with respect to the time variable t, represents the mean value of the k-th Gaussian distribution with respect to position, represents the covariance matrix of the k-th Gaussian distribution, The variance of the time variable t, The covariance between time and position, The covariance of the positions, yes It should be understood that the above splitting calculation is a prior art in this field, so it will not be described in detail.

[0132] 1. Calculate the posterior of each Gaussian regression model at time t:

[0133] ;

[0134] in, represents the posterior weight of the kth Gaussian component at a given time t.

[0135] 2. Calculate the conditional mean and covariance of each component:

[0136] ;

[0137] in, represents the output mean prediction of the k-th Gaussian component at time t, represents the output covariance prediction of the k-th Gaussian component at time t.

[0138] 3. Calculate the weighted average prediction result:

[0139] ;

[0140] in, Represents the expected value of the predicted output at a given time t.

[0141] Step 6: Introduce model predictive control.

[0142] S6-1: Establish a state space model of the robot arm to describe the dynamic evolution of the system under the influence of the current state and control input. The model is formally expressed as:

[0143] ;

[0144] in, represents the system state at time t, represents the control input, i.e., the input control quantity of the robot controller. The present invention does not limit the specific control input quantity. f() represents the state transfer function, Indicates the system status at the next moment.

[0145] S6-2: Optimal path planning is achieved between the robot's current position and the reference trajectory by constructing an objective function. Combined with real-time controller updates, the robot's motion behavior is dynamically adjusted to suit the task requirements. In this embodiment, the following objective function is preferred. In other feasible embodiments, other objective functions may be selected while ensuring optimal results, and are within the scope of this invention.

[0146] ;

[0147] in, represents the objective function, represents the position of the end of the robotic arm at time t+k, It represents the target position of the end of the robot arm, that is, the position that the robot arm wants to reach that is known in advance. and Represents the weight coefficient, and its specific value is generally adjusted according to different tasks. When it is larger, the objective function will give priority to the shorter path of the robot arm. When it is large, the system will give priority to reducing the changes in the control input to make the trajectory smoother. H represents the number of steps predicted by the controller at the current time t into the future.

[0148] S6-3: Constraints that need to be considered when planning the robot path:

[0149] 1. Avoid collisions:

[0150] ;

[0151] in, Represents the state of the end of the robotic arm in the next k steps, that is, the position, It represents the hand trajectory predicted by the Gaussian mixture model. It should be understood that the hand trajectory and the position of the end of the robotic arm are both three-dimensional vectors. Indicates the safety distance threshold, Indicates that all time steps within the forecast horizon must satisfy this constraint.

[0152] 2. Input of control quantity

[0153] For the control input of the robotic arm, the control input should meet the following constraints:

[0154] ;

[0155] in, Represents the input of the control quantity of the robot arm, Indicates the minimum value of the input control quantity, Indicates the maximum value of the input control quantity.

[0156] 3. Constraints on the angle and angular velocity of the robotic arm

[0157] For the entire robotic arm, the angle and speed of each joint of the robotic arm are subject to physical limitations, that is,

[0158] ;

[0159] ;

[0160] in, Respectively represent the minimum joint angle and the maximum joint angle during the movement of the robot arm, , Indicates the joint angle and angular velocity of the robot arm; , They represent the minimum angular velocity and the maximum angular velocity during the movement of the robotic arm.

[0161] 4. Constraints on the kinematics of the robotic arm

[0162] When the robot arm is planning its path, the position of the end effector must comply with the positive kinematic constraints of the robot arm, namely: ;

[0163] in, represents the forward kinematics of the robotic arm, Indicates the angles of each joint of the robotic arm, Indicates the position of the end effector of the robot arm.

[0164] To sum up, the entire optimization problem is:

[0165] Min J;

[0166] ;

[0167] By solving the optimization problem, the final path of the robotic arm is obtained. It should be understood that the solution process of the optimization problem is achievable in the prior art and the present invention does not specifically limit this solution process. It should also be understood that in other feasible embodiments, based on the above, combining other constraints or incorporating other feasible constraints also falls within the scope of protection of the present invention.

[0168] In summary, this invention, through the aforementioned technical solutions, effectively addresses the potential collision risk caused by the robotic arm's inability to perceive the human's movement intentions in real time during human-robot collaborative tasks. By modeling and predicting the human hand's motion trajectory, the system can plan the robotic arm's path in advance, enabling rapid response and avoidance to the human's dynamic behavior, thus enhancing the safety and intelligence of human-robot collaboration.

[0169] Example 2:

[0170] An embodiment of the present invention provides a system based on the above-mentioned path planning method, including: a human hand motion key point data acquisition module, a joint variable construction module, a Gaussian mixture model construction module, a training module and a path planning module that are connected in sequence or interconnected.

[0171] Among them, the human hand motion key point data acquisition module is used to collect human body motion data in a continuous time series to obtain the position set of human hand motion key points.

[0172] The joint variable construction module is used to construct joint variables and observation data sets based on joint variables. The joint variables are composed of time t and the spatial position of the hand at the corresponding moment. composition.

[0173] The Gaussian mixture model construction module is used to construct a Gaussian mixture model, that is, the distribution of joint variables is represented by the weighted sum of multiple Gaussian distributions.

[0174] A training module is used to train the Gaussian mixture model using the observation data set to determine the model parameters of the Gaussian mixture model , are the weight, mean vector and covariance matrix of the kth Gaussian distribution respectively.

[0175] The path planning module is used to introduce model predictive control, that is, to establish a state-space model of the robotic arm, then construct an objective function based on the state-space model, and introduce the Gaussian mixture model's prediction result of the human hand trajectory into the constraint condition to achieve the optimal path planning between the robotic arm and the reference trajectory; wherein, the Gaussian mixture model is used to predict the human hand trajectory of the robotic arm at a future moment, and then a collision-avoidance constraint term is constructed with the position state of the robotic arm end at the same moment obtained based on the state-space model.

[0176] It should also be understood that the specific implementation process of each module please refer to the above method content, the present invention will not go into details here, and the division of the above functional modules is only for illustration. In some embodiments, some functional modules can be merged, and some functional modules can be split. Each functional module can be implemented in software or hardware or a combination of software and hardware. Among them, the software and hardware equipment includes but is not limited to general electronic terminals, programmable gate arrays, digital signal processors, microprocessors and their corresponding programming or burning software.

[0177] Example 3:

[0178] An embodiment of the present invention provides a computer device, comprising: one or more processors; and a memory storing a computer program; wherein the processor calls the computer program to implement:

[0179] A step of a path planning method based on Gaussian mixture model and model predictive control.

[0180] Specific implementation:

[0181] S1: Collect human motion data in a continuous time series to obtain the position set of key points of human hand motion;

[0182] S2: Construct joint variables and observation data sets based on joint variables, where the joint variables are composed of time t and the spatial position of the hand at the corresponding moment composition;

[0183] S3: Construct a Gaussian mixture model, that is, the distribution of the joint variable is represented by the weighted sum of multiple Gaussian distributions;

[0184] S4: Using the observation data set to train the Gaussian mixture model to determine the model parameters of the Gaussian mixture model , are the weight, mean vector and covariance matrix of the k-th Gaussian distribution respectively;

[0185] S5: Introduce model predictive control, that is, establish a state space model of the robotic arm, then construct an objective function based on the state space model, and introduce the Gaussian mixture model's prediction result of the human hand trajectory into the constraint condition to achieve the optimal path planning between the robotic arm and the reference trajectory; wherein, the Gaussian mixture model is used to predict the human hand trajectory of the robotic arm at the future moment, and then a collision avoidance constraint item is constructed with the position state of the end of the robotic arm at the same moment obtained based on the state space model.

[0186] For the specific implementation process of each step, please refer to the description of the aforementioned embodiment of the path planning method based on Gaussian mixture model and model predictive control.

[0187] In some embodiments, as Figure 2 As shown, the electronic components of computer equipment include:

[0188] Processor 1600 may be a central processing unit (CPU), other general-purpose processors, digital signal processors (DSP), application-specific integrated circuits (ASIC), field-programmable gate arrays (FPGA), other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. A general-purpose processor may be a microprocessor or any conventional processor. Processor 1600 is configured to execute relevant programs to implement the technical solutions provided by the embodiments of the present invention.

[0189] Memory 1700 can be implemented in the form of a read-only memory (ROM), a static storage device, a dynamic storage device, or a random access memory (RAM). Memory 1700 can store an operating system and other application programs. When the technical solutions provided in the embodiments of this specification are implemented through software or firmware, the relevant program code is stored in memory 1700, and the processor 1600 calls and executes the algorithm program of the path planning method based on the Gaussian mixture model and model predictive control according to the embodiment of the present invention.

[0190] The input / output interface 1800 is used to implement information input and output.

[0191] The communication interface 1900 is used to realize communication interaction between this device and other devices. Communication can be achieved through wired means (such as USB, network cable, etc.) or wireless means (such as mobile network, WIFI, Bluetooth, etc.).

[0192] The bus 2000 transmits information between various components of the device (eg, the processor 1600 , the memory 1700 , the input / output interface 1800 , and the communication interface 1900 ).

[0193] The processor 1600 , the memory 1700 , the input / output interface 1800 , and the communication interface 1900 are connected to each other in communication within the device via the bus 2000 .

[0194] Example 4:

[0195] An embodiment of the present invention provides a computer-readable storage medium storing a computer program, wherein the computer program is invoked by a processor to implement:

[0196] A step of a path planning method based on Gaussian mixture model and model predictive control.

[0197] Specific implementation:

[0198] S1: Collect human motion data in a continuous time series to obtain the position set of key points of human hand motion;

[0199] S2: Construct joint variables and observation data sets based on joint variables, where the joint variables are composed of time t and the spatial position of the hand at the corresponding moment composition;

[0200] S3: Construct a Gaussian mixture model, that is, the distribution of the joint variable is represented by the weighted sum of multiple Gaussian distributions;

[0201] S4: Using the observation data set to train the Gaussian mixture model to determine the model parameters of the Gaussian mixture model , are the weight, mean vector and covariance matrix of the k-th Gaussian distribution respectively;

[0202] S5: Introduce model predictive control, that is, establish a state space model of the robotic arm, then construct an objective function based on the state space model, and introduce the Gaussian mixture model's prediction result of the human hand trajectory into the constraint condition to achieve the optimal path planning between the robotic arm and the reference trajectory; wherein, the Gaussian mixture model is used to predict the human hand trajectory of the robotic arm at the future moment, and then a collision avoidance constraint item is constructed with the position state of the end of the robotic arm at the same moment obtained based on the state space model.

[0203] For the specific implementation process of each step, please refer to the description of the aforementioned embodiment of the path planning method based on Gaussian mixture model and model predictive control.

[0204] The readable storage medium is a computer-readable storage medium, which can be an internal storage unit of the software and hardware device described in any of the aforementioned embodiments, such as a hard disk or memory of a controller. The readable storage medium can also be an external storage device of the controller, such as a plug-in hard disk equipped on the controller, a smart memory card (Smart Media Card, SMC), a secure digital (Secure Digital, SD) card, a flash card (Flash Card), etc. Furthermore, the readable storage medium can also include both an internal storage unit of the controller and an external storage device. The readable storage medium is used to store the computer program and other programs and data required by the controller. The readable storage medium can also be used to temporarily store data that has been output or is to be output.

[0205] Based on this understanding, the technical solution of the present invention, or the portion that contributes to the prior art, or all or part of the technical solution, can be embodied in the form of a software product. This computer software product is stored in a storage medium and includes instructions for causing a computer device (such as a personal computer, server, or network device) to perform all or part of the steps of the methods described in various embodiments of the present invention. The aforementioned readable storage medium includes various media capable of storing program code, such as a USB flash drive, a mobile hard drive, a read-only memory (ROM), a random access memory (RAM), a magnetic disk, or an optical disk.

[0206] Those skilled in the art will appreciate that the embodiments of the present application can be provided as methods, systems, or computer program products. Therefore, the present application can adopt the form of a complete hardware embodiment, a complete software embodiment, or an embodiment combining software and hardware. Moreover, the present application can adopt the form of a computer program product implemented on one or more computer-readable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code. The present application is a flow chart according to the method, device (system), and computer program product of the embodiment of the present application and / or the instructions executed by the processor to generate a device for realizing the function specified in one flow chart or multiple flows and / or one box or multiple boxes of the block diagram. These computer program instructions can also be stored in a computer-readable memory that can guide a computer or other programmable data processing device to work in a specific manner, so that the instructions stored in the computer-readable memory generate a product comprising an instruction device, which realizes the function specified in one flow chart or multiple flows and / or one box or multiple boxes of the block diagram. These computer program instructions may also be loaded onto a computer or other programmable data processing device so that a series of operational steps are executed on the computer or other programmable device to produce a computer-implemented process, whereby the instructions executed on the computer or other programmable device provide steps for implementing the functions specified in one or more processes in the flowchart and / or one or more boxes in the block diagram.

[0207] It should be emphasized that the examples described in the present invention are illustrative rather than restrictive. Therefore, the present invention is not limited to the examples described in the specific embodiments. Any other embodiments derived by those skilled in the art based on the technical solution of the present invention that do not depart from the purpose and scope of the present invention, whether modified or replaced, also fall within the scope of protection of the present invention.

Claims

1. A path planning method based on a Gaussian mixture model and model predictive control, characterized by: The following steps are involved: S1: Collect human motion data in a continuous time series to obtain the position set of key points of human hand motion as the human hand spatial position set to represent the human hand trajectory; S2: Construct joint variables and observation data sets based on joint variables, where the joint variables are composed of time t and the spatial position of the hand at the corresponding moment composition; S3: Construct a Gaussian mixture model, that is, the distribution of the joint variable is represented by the weighted sum of multiple Gaussian distributions; S4: Using the observation data set to train the Gaussian mixture model to determine the model parameters of the Gaussian mixture model , are the weight, mean vector and covariance matrix of the k-th Gaussian distribution respectively; S5: Introducing model predictive control, i.e., establishing a state space model of the robotic arm, constructing an objective function based on the state space model, and introducing the hand trajectory prediction results of the Gaussian mixture model into the constraint conditions to achieve optimal path planning between the robotic arm's current position and the reference trajectory; The Gaussian mixture model is used to predict the trajectory of the human hand of the robotic arm at a future moment, and then a collision avoidance constraint term is constructed based on the position state of the end of the robotic arm at the same moment obtained based on the state space model.

2. The method according to claim 1, wherein: The collision avoidance constraint term is expressed as: ； in, It indicates the state of the end of the manipulator in the next k steps based on the state space model, i.e., the position, Indicates the use of Gaussian mixture model to predict the hand trajectory in the next k steps, that is, the position of the key points of the hand movement, t is the current time, H is the number of time steps predicted from the current time t to the future, Indicates the safety distance threshold, represents any time step within the forecast horizon.

3. The method according to claim 2, wherein: The objective function is expressed as: ； in, represents the objective function, represents the target state of the end of the robotic arm in the next k steps, and Both represent weight coefficients, is the input control quantity of the robot controller in the next k steps.

4. The method according to claim 2, wherein: The constraints in the model predictive control also include a combination of one or more of the following: input control quantity constraints, robot arm angle and angular velocity constraints, and robot arm kinematic constraints.

5. The method according to claim 1, wherein: The process of using the Gaussian mixture model to predict the hand trajectory at a future moment is as follows: For the prediction time t, the mean and covariance of the trained Gaussian mixture model parameters are first decomposed: ； in, represents the mean vector of the k-th Gaussian distribution, represents the mean of the k-th Gaussian distribution with respect to the time variable t, represents the mean value of the k-th Gaussian distribution about position P, represents the covariance matrix of the k-th Gaussian distribution, represents the variance of the time variable t, represents the covariance between time t and position P, represents the covariance of position P, yes The transposed matrix of Then calculate the posterior of each Gaussian regression model at time t: ； in, represents the posterior weight of the k-th Gaussian distribution at a given time t, The mean is , the covariance is The Gaussian probability density function, The mean is , the covariance is Gaussian probability density function of ; For the j The weight corresponding to the Gaussian distribution; Then, calculate the conditional mean and covariance of each Gaussian distribution: ； in, represents the output mean prediction of the k-th Gaussian distribution at time t, represents the output covariance prediction of the k-th Gaussian component at time t; Finally, calculate the result of the weighted average prediction: ； in, It represents the expected value of the predicted output at a given time t, that is, the hand trajectory at the predicted time t, and K is the number of Gaussian distributions.

6. The method according to claim 1, wherein: In step S4, the Gaussian mixture model is trained using the observation data set, and the expectation maximization algorithm EM is introduced to learn the parameters of the Gaussian mixture model. , as follows: First randomly initialize the parameters to obtain the parameters after initialization , that is, k The initial values ​​of the weights, mean vectors, and covariance matrices of the Gaussian distributions; Then use the expectation maximization algorithm EM to iterate until the iteration termination condition is met to obtain the parameters of the final Gaussian mixture model, where step E: calculate each sample data point The posterior probability of belonging to the k-th Gaussian distribution; Step M: Update the parameters of the Gaussian mixture model using the posterior probability; Among them, the posterior probability formula is: ； in, Indicates the nth sample data point in the i-th iteration The posterior probability from the kth Gaussian component; 、 They represent the kth Gaussian distribution, the kth Gaussian distribution and the kth Gaussian distribution in the i-th iteration respectively. j The weights of the Gaussian distribution; Indicates the k-th Gaussian distribution for the sample data point in the i-th iteration The probability density of Indicates the j-th Gaussian distribution for the sample data point in the i-th iteration The probability density of and Respectively represent the mean vectors of the kth and jth Gaussian distributions in the i-th iteration, and Respectively represent the covariance matrices of the kth and jth Gaussian distributions in the i-th iteration, K represents the total number of Gaussian distributions in the Gaussian mixture model, Represents the nth sample data point, that is, the location parameter of the joint variable at a moment in the observation data set as a sample data point; The parameter update formula of the Gaussian mixture model in the M step is as follows: ； ； ； ； in, represents the number of valid samples of the kth Gaussian distribution in the i+1th iteration, and N represents the number of samples; The weight of the k-th Gaussian distribution at the i+1-th iteration; represents the mean vector of the k-th Gaussian distribution in the i+1-th iteration; represents the covariance matrix of the k-th Gaussian distribution in the i+1-th iteration, and T is the matrix transpose symbol.

7. The method according to claim 1, wherein: The Gaussian mixture model constructed in step S3 is expressed as: ； Where K is the number of Gaussian distributions in the Gaussian mixture model, and the weight Satisfy the normalization conditions: ； The mean is And the covariance matrix is The probability density function of , exists: ； Where d is the joint variable The number of dimensions, T is the matrix transpose.

8. A system based on the method according to any one of claims 1 to 7, characterized in that: include: The human hand motion key point data acquisition module is used to collect human body motion data in a continuous time series and obtain the position set of human hand motion key points; The joint variable construction module is used to construct joint variables and observation data sets based on joint variables. The joint variables are composed of time t and the spatial position of the hand at the corresponding moment. composition; Gaussian mixture model construction module, used to construct a Gaussian mixture model, that is, the distribution of joint variables is represented by the weighted sum of multiple Gaussian distributions; A training module is used to train the Gaussian mixture model using the observation data set to determine the model parameters of the Gaussian mixture model , are the weight, mean vector and covariance matrix of the k-th Gaussian distribution respectively; A path planning module is used to introduce model predictive control, that is, to establish a state-space model of the robotic arm, then construct an objective function based on the state-space model, and introduce the Gaussian mixture model's prediction of the human hand trajectory into the constraints to achieve optimal path planning between the robotic arm's current position and the reference trajectory; The Gaussian mixture model is used to predict the trajectory of the human hand of the robotic arm at a future moment, and then a collision avoidance constraint term is constructed based on the position state of the end of the robotic arm at the same moment obtained based on the state space model.

9. A computer device, characterized in that: include: one or more processors; and a memory storing a computer program; The processor calls a computer program to implement: The steps of a path planning method based on a Gaussian mixture model and model predictive control as described in any one of claims 1-7.

10. A computer-readable storage medium, characterized in that: A computer program is stored, which is called by a processor to implement: The steps of a path planning method based on a Gaussian mixture model and model predictive control as described in any one of claims 1-7.

Images