Mechanical arm path re-planning method based on human hand trajectory prediction

Abstract

The invention discloses a mechanical arm path re-planning method based on human hand trajectory prediction, which comprises the following steps: step 1, acquiring an image of a motion trajectory of a human hand of a worker cooperating with a mechanical arm in a three-dimensional space, and recognizing a motion pose of the human hand according to the image; step 2, according to the motion pose of the human hand obtained in the step 1, modeling the behavior and track of the human hand based on a hidden Markov model, and then predicting the future motion track of the human hand by learning historical data; and 3, an improved fast exploration random tree algorithm is used for generating a mechanical arm path from the current position to the target position and avoiding the future movement track, predicted in the step 2, of the human hand for the mechanical arm. According to the mechanical arm path re-planning method based on human hand prediction, the safety of human-computer interaction is improved, and the adaptability and the response speed of a robot system are remarkably improved.

Description

Technical Field

[0001] The present invention relates to the field of robot arm control, and in particular to a robot arm path replanning method based on hand trajectory prediction. Background Art

[0002] With the continuous development of artificial intelligence and robotic arm control technology, the role of robotic arms in daily life and industrial applications is becoming more and more important. Figure 1 ,Human-machine collaboration system has greatly helped improve human work efficiency in the fields of manufacturing, medical, and service industries. However, this collaboration model also brings new safety challenges, especially when humans and robotic arms coexist in the same workspace. Ensuring the safety of human workers has become the primary task in designing efficient human-machine interaction systems.

[0003] At present, traditional human-machine interaction safety strategies mainly rely on fixed safety measures, such as safety fences and emergency stop buttons, which often limit the interactive potential of robotic arms and humans. As the complexity of the environment increases, traditional methods are increasingly unable to meet the needs of efficient and flexible operations. For example, in a factory, robotic arms need to work closely with human colleagues in a constantly changing environment. At this time, physical isolation and simple emergency stop buttons are not enough to ensure safety. Furthermore, existing technologies have many shortcomings in dealing with human trajectory prediction and robotic arm path planning in dynamic environments. Although optical tracking, radar and other forms of sensors can provide real-time data, these technologies are usually unable to accurately predict the long-term trajectory of human actions or perform effective path planning in complex environments.

[0004] Therefore, when humans and robotic arms work together, there are the following technical problems: 1) Safety risks in dynamic environments. In traditional human-computer interaction environments, dynamic and unstructured work scenarios often lead to safety risks, especially when humans and robotic arms need to work closely together in the same space. Existing technologies lack effective prediction and real-time response mechanisms, and cannot fully predict human movements and paths, thereby increasing the risk of collision. 2) Path planning efficiency. In complex operating environments, traditional path planning methods often cannot meet the needs of rapid response, resulting in slow response and low efficiency of robotic arms when avoiding obstacles or adjusting paths. 3) Flexibility and efficiency of human-computer collaboration. Due to the lack of effective prediction and adjustment mechanisms, existing human-computer interaction systems often cannot achieve high efficiency and high flexibility while ensuring safety. Summary of the invention

[0005] In order to solve the current technical problem that there is no effective way to predict the motion trajectory of collaborative workers to achieve robot arm avoidance path planning, the present invention provides a robot arm path replanning method based on human hand trajectory prediction, which can replan a collision-free path for the robot arm by predicting the future behavior of humans.

[0006] In order to achieve the above technical purpose, the technical solution of the present invention is:

[0007] A method for replanning a robot arm path based on hand trajectory prediction comprises the following steps:

[0008] Step 1, collecting images of the motion trajectory of the human hand of the worker cooperating with the robot arm in three-dimensional space, and identifying the motion posture of the human hand based on the images;

[0009] Step 2: According to the motion posture of the human hand obtained in step 1, the motion posture of the human hand is modeled based on the hidden Markov model, and then the future motion trajectory of the human hand is predicted by learning from historical data;

[0010] In step 3, an improved fast exploration random tree algorithm is used to generate a robotic arm path from the current position to the target position for the robotic arm while avoiding the future movement trajectory of the human hand predicted in step 2.

[0011] The method, step 1 comprises:

[0012] The robot arm cooperates with the human hand of the worker to capture images at a fixed frequency based on the depth camera; then the captured images are sent to the extraction model for extracting the motion posture of the human hand, so as to obtain the motion posture of the human hand.

[0013] The method, in step 1, further comprises a denoising step after the extraction model extracts the motion posture of the human hand:

[0014] The sliding average filter method is used to perform denoising on the x-axis data of the robot's base coordinate system. , the following formula is used to perform sliding window filtering to obtain the denoised data :

[0015] ;

[0016] in Indicates the x-axis position of the human hand at time t in the base coordinate system of the robot arm, represents the time point of collecting the hand position, and k represents half of the filter window size;

[0017] Then based on the same calculation formula, substitute or To replace Filter to obtain denoised data and ,in Indicates the y-axis position of the human hand at time t in the base coordinate system of the robot arm, represents the z-axis position of the human hand at time t in the base coordinate system of the robot arm, and is expressed as To represent the position of the human hand at time t in the robot base coordinate system, that is, the observation data point.

[0018] The method, in step 1, further comprises the step of processing outliers after the step of denoising:

[0019] Use the median absolute deviation to identify and correct outliers: first calculate the median of the three coordinate values ​​of each observation data point of the collected human motion posture, then calculate the absolute deviation of each observation data point from the median, and find the median of the absolute deviation :

[0020] ;

[0021] Where median(o) represents the median of the data set, and median() represents the function used to calculate the median; then outlier determination is performed: when the deviation of a data point exceeds three times When the value of is greater than , the data point is considered to be an outlier and is replaced by the average of the two adjacent non-outlier points.

[0022] In the method, in step 2, modeling the motion posture of the human hand based on the hidden Markov model includes:

[0023] Modeling of the motion posture of the human hand based on the hidden Markov model using triples :

[0024] ;

[0025] in represents the initial state probability distribution, , It means that when the implicit variable The value is The probability of , represents the set of all possible values ​​of the implicit variable, , Represents the total number of all possible values ​​of the hidden variable, that is, the possible number of hidden states; represents the state transfer matrix, The elements in are , ,in represents the probability of transitioning from the state at the previous moment to the state at the current moment, The r and s in it represent the rows and columns of the state transfer matrix respectively. For in time The implicit variable of , represents a sequence of states, ; represents the observation state matrix, The elements in are , , Indicated in Observation status In the implicit variable The probability of generation under the condition, v represents the possible value of the observed variable, v V, V represents the set of all possible values ​​of the observed variable, , Represents the number of possible observation states.

[0026] The method described, the state sequence Each state variable in Corresponding to the observation sequence An observed variable in , the observation sequence , T is a given time length, each in the observation sequence That is, in time Observation data points ; and any implicit variable , any observed state .

[0027] The method, after establishing the HMM, also includes taking the collected observation data point set as the observation sequence Input into HMM, train HMM, and thus obtain the step of HMM parameter information.

[0028] In the method, in step 2, predicting the future movement trajectory of the human hand includes:

[0029] For The observed value obtained at the time , by obtaining To predict the observation value in the future, h represents the time interval; All state values ​​in are defined as a complete event group, and the following formula is obtained through the total probability formula:

[0030] ;

[0031] in Defined by the observation state matrix;

[0032] for ,Will The values ​​of all states in are defined as a complete event group, which is obtained by the total probability formula:

[0033] ;

[0034] in Obtained from the state transfer matrix;

[0035] and ;

[0036] and = , and is represented by the state transfer matrix, It is represented by the elements in the previous t+h-2 state transfer matrices; thus calculating , that is, calculated from arrive The distribution of the obstacle C is set as the expected value of the distribution. obs The location.

[0037] In the method, in step 3, the robot arm path is to solve the minimum value of the optimization problem expressed by the following formula:

[0038] ;

[0039] in, Indicates that the robotic arm that can be fully planned is arrive The path function, is the initial position of the robot arm, is the target position to which the robot arm will eventually move; min represents the minimum value, is the collision-free area, Represents all sampled points on the path; Represents the set of path functions that meet the above constraints.

[0040] The method described uses an improved fast exploration random tree algorithm to The solution includes:

[0041] In the working space of the robot arm Initialize two trees in and ,in from Start iteration, from Start the iteration, and in each iteration Random sampling to obtain sampling points , find the tree The closest point is defined as , and then get , where the steering function refers to towards Direction is turned to determine the direction of the new node, and the step size refers to and The maximum distance between them; then connect and , when an obstacle C appears on the connecting line obs , it means a collision occurs, and the system returns to , then exclude the original Find the distance The nearest point is taken as the new , until no C appears on the connecting line obs After that, The node is added to the tree, and then the iteration is repeated until and Connected to each other.

[0042] The technical effect of the present invention is that a hidden Markov model is used to model and learn the time series data of human actions, so that the future position and action of humans can be predicted. This prediction allows the robot arm to adjust its action plan more intelligently, avoid potential conflicts, and optimize the workflow. In addition, the present invention also uses an improved rapid exploration random tree (RRT) algorithm, namely RRT-Connect, for real-time path planning. The algorithm effectively shortens the time for calculating the path and improves the efficiency of path planning by constructing a tree structure that grows rapidly toward the target area. When combined with the prediction data provided by the hidden Markov model, RRT-Connect can optimize the motion trajectory of the robot arm while ensuring safety, thereby reducing job interruptions and improving job efficiency. By combining these two technologies, the robot arm path replanning method based on hand prediction proposed in the present invention not only improves the safety of human-machine interaction, but also significantly enhances the adaptability and response speed of the robot system. This innovative safety framework provides a new solution for future human-machine collaborative environments, and is particularly suitable for complex tasks and environments that require close cooperation between robot arms and humans. BRIEF DESCRIPTION OF THE DRAWINGS

[0043] Figure 1 It is a schematic diagram of the working scene of human-machine collaboration handled by the present invention.

[0044] Figure 2 This is a schematic diagram of human feature extraction using the MediaPipe framework.

[0045] Figure 3 It is a system scheme diagram of the present invention.

[0046] Figure 4 It is the correspondence between the observation sequence and the state sequence in the hidden Markov model of the present invention.

[0047] Figure 5 It is a schematic diagram of the RRT-Connet algorithm model in the present invention.

[0048] Figure 6 The following are comparison diagrams of the results of the present invention and other traditional algorithms. (a) is a schematic diagram of the results of LazyRRT; (b) is a schematic diagram of the results of RRTextend; (c) is a schematic diagram of the results of POA-RRT-Connect, i.e., the present invention; and (d) is a schematic diagram of the results of RRTStar. DETAILED DESCRIPTION

[0049] The embodiments of the present invention are further described below in conjunction with the accompanying drawings.

[0050] See also Figure 3 , a robot arm path replanning method based on hand trajectory prediction provided in this embodiment includes the following steps:

[0051] S1, Definition of human position information: In the joint space of the robot arm, the coordinate system of the robot arm's base coordinates is used as the reference coordinate system to collect the human hand position data. The position of the human hand in the robot arm's base coordinate system is defined as ,in Respectively represent the hands coordinate, Coordinates and coordinate, Indicates the time point when the camera captures the position of the hand.

[0052] S2, collection of human position information: Use a depth camera to collect the position information of a moving human body at a fixed frequency, and transmit the collected information to an extraction model for extracting the motion posture of the human hand, and use the extraction model to obtain the position of the human body in the robot arm base coordinate system. In this embodiment, the depth camera used is Intel's Realsense camera based on the structured light solution, and other depth cameras such as Tof, binocular imaging, etc. can also be used as needed. Figure 2The extraction model for extracting the motion posture of human hands uses the MediaPipe framework. The MediaPipe framework is an open source cross-platform multimedia processing framework developed by Google. It includes a series of pre-trained models and tools, which can realize target tracking, gesture recognition, face recognition and other functions. Other models for extracting the motion posture of human hands can also be used as needed.

[0053] S3, processing the collected human motion data: using the sliding average filter method to smooth the collected human motion data to reduce noise. , , , the filtered data , , The following formula can be used to calculate:

[0054] ;

[0055] in It represents half of the filter window size used for sliding average filtering, which means that the window covers from Time has come In this way, the center of the window is located at time, and the window size is , thus ensuring the symmetry and uniformity of the data smoothing process. When setting the window size, for data that changes quickly, a smaller window is needed to ensure that the algorithm can respond to data changes in a timely manner. For data that changes slowly, a larger window is needed to achieve smoother results. The specific value can be adjusted according to actual usage needs. The same formula is also used for and The coordinate data is denoised. or replace That's it.

[0056] After removing the noise from the collected data, the outliers in the data are processed and the median absolute deviation is used to process the collected data to identify and correct the outliers: first calculate the median of the three coordinate values ​​of each observation data point of the collected human body motion posture, then calculate the absolute deviation of each observation data point from the median, and find the median of the absolute deviation :

[0057] ;

[0058] Where median(o) represents the median of the data set, and median() represents the function used to calculate the median; then outlier determination is performed: when the deviation of a data point exceeds three times When the value of is greater than , the data point is considered to be an outlier and is replaced by the average of the two adjacent non-outlier points.

[0059] S4, see Figure 4 , define the observation sequence and state sequence: within a given time length T, the recorded hand motion data is defined as the observation sequence , expressed as Each of these For at the moment The observation data points mentioned above Accordingly, define a state sequence , expressed as , where each For in time Each latent variable corresponds to an observed variable.

[0060] S5, define the value set of state and observation variables: define the set of all possible values ​​of the observation variables as ,in ,in Represents the number of possible types of observed states. Define the set of all possible values ​​of the state variable as ,in ,in represents the possible number of hidden states. Any hidden state , any observed state .

[0061] S6, establish hidden Markov model: The hidden Markov model can be defined by a triple: .in represents the initial state probability distribution, . It means that when the state variable The value is The probability of , represents the set of all possible values ​​of the state variable, , Represents the total number of all possible values ​​of the hidden variable, that is, the possible number of hidden states. The value of the hidden variable is learned by the hidden Markov model. , ,in represents the probability of transitioning from the state at the previous moment to the state at the current moment, The r and s in it represent the rows and columns of the state transfer matrix respectively. For in time The implicit variable of , represents a sequence of states, ; represents the observation state matrix, The elements in are , , Indicated in Observation status In the implicit variable The probability of generation under the condition, v represents the possible value of the observed variable, v V, V represents the set of all possible values ​​of the observed variable, , Represents the number of possible observation states.

[0062] Then the previously collected data with a time length of T is input into the hidden Markov model for training, so as to obtain the parameter information of the hidden Markov model.

[0063] S7, using hidden Markov model for prediction: taking the hand sequence as the observation sequence, and predicting the hand trajectory in the future by observing the historical human motion trajectory. The observation value at a given time , to infer its observation value in the future, we first need to obtain , where h represents the time interval. All state values ​​in are defined as a complete event group, and the following formula can be obtained through the total probability formula:

[0064] ;

[0065] in is defined by the observation state matrix. ,Will The values ​​of all states in are defined as a complete event group, which can be obtained through the total probability formula:

[0066] ;

[0067] in It is obtained from the state transfer matrix. , it can be seen from the fact that the hidden Markov model needs to satisfy the homogeneous Markov property and the observation independence assumption, ,for Further deduction can be obtained:

[0068] ;

[0069] in From the homogeneous Markov theorem, we know that its value is equal to , and can be represented by the state transfer matrix, It can be represented by the elements in the previous t+h-2 state transfer matrices. When arrive According to the distribution, the expected value of the distribution is used as a relatively accurate prediction value, and the prediction value is set as the location of the obstacle. .

[0070] S8, see Figure 5 , define the robot arm workspace: define the space where the robot arm works as space, where the collision-free region is defined as , the space where the obstacle is located is obtained in S7 , the initial position of the robot arm is defined as , and the target position to which the robot arm will eventually move is defined as .

[0071] S9, set the path planning task: The task of the robot path planning is to find a path from arrive The optimal collision-free path. Path function It is represented as a path that can be completely planned. All sampled points on the path are defined as ,in The path function satisfies the following constraints: and , ,in Indicates that the robotic arm that can be fully planned is arrive The set of path functions that meet the above constraints is defined as , solving the optimal path is to solve the minimum value of the following optimization problem :

[0072] ;

[0073] S10, robot path replanning. First, two trees are initialized in the robot workspace. The two trees are and , from Start iteration, from Start the iteration, and in each iteration Random sampling in space , find the point in the tree closest to the sampling point and define it as , and then get , where the steering function refers to towards Direction is turned to determine the direction of the new node, and the step size refers to and The maximum distance between them. Reconnect and If there is no obstacle on the connecting line between the two , which means there are no obstacles If a collision occurs, Add a node to the tree. If an obstacle appears on the connecting line C obs , it means a collision occurs, and the system returns to , then exclude the original Find the distance The nearest point is taken as the new , until no C appears on the connecting line obs After that, The nodes are added to the tree, and the process is iterated until the two trees are connected. When the two trees are connected, it means that the robot has found a collision-free trajectory from the starting point to the end point.

[0074] The following is a verification analysis by giving some specific examples, by setting the following experimental parameters:

[0075] 1) The search area of ​​the robot arm is 200×200×300cm 3 in the space.

[0076] 2) The initial position of the robot arm is planned to be [150, 400, 400].

[0077] 3) The target position of the robot arm is [250,450,450].

[0078] Results Figure 6And the following table 1, where POA-RRT-Connect represents the method adopted in this embodiment. It can be seen from the figure that this method can quickly find the path and is suitable for high-dimensional complex environments. Although there is also the problem that the path planning may not be the shortest path, since the present invention is to ensure that the robot arm collides with the human body as much as possible, even if the planned path is slightly longer, it can achieve safety redundancy to a certain extent to avoid collisions to the greatest extent. RRTextend is an extension based on the traditional RRT algorithm, aiming to improve the efficiency and path quality in the path extension process. RRTextend improves the performance of RRT in complex environments by optimizing the tree expansion strategy, improving the collision detection method and introducing an intelligent sampling mechanism, but the implementation of RRTextend is relatively complex and relies more on domain knowledge. RRT Star is based on maintaining the efficient exploration capability of RRT, and by introducing a path optimization mechanism, the generated path is not only feasible, but also with the increase of iterations, the path quality gradually improves and approaches the optimal, but its computational overhead is large and the parameter adjustment is complex, resulting in the overall implementation being too complex. The LazyRRT algorithm is an improvement on the traditional RRT algorithm, aiming to reduce the number of collision detections, thereby improving the overall efficiency of the algorithm. LazyRRT speeds up the path planning process by delaying collision detection and reducing unnecessary collision checks, but its planning time is long and may even result in the inability to plan a path.

[0079] Table 1 Comparison of different algorithms .

Claims

1. A robot arm path replanning method based on hand trajectory prediction, characterized in that: The following steps are involved: Step 1, collecting images of the motion trajectory of the human hand of the worker cooperating with the robot arm in three-dimensional space, and identifying the motion posture of the human hand based on the images; Step 2: According to the motion posture of the human hand obtained in step 1, the motion posture of the human hand is modeled based on the hidden Markov model, and then the future motion trajectory of the human hand is predicted by learning from historical data; In step 3, an improved fast exploration random tree algorithm is used to generate a robotic arm path from the current position to the target position for the robotic arm while avoiding the future movement trajectory of the human hand predicted in step 2.

2. The method according to claim 1, characterized in that The step 1 comprises: The robot arm cooperates with the human hand of the worker to capture images at a fixed frequency based on the depth camera; then the captured images are sent to the extraction model for extracting the motion posture of the human hand, so as to obtain the motion posture of the human hand.

3. The method according to claim 2, characterized in that In the step 1, after the extraction model extracts the motion posture of the human hand, a denoising step is also included: The sliding average filter method is used to perform denoising on the x-axis data of the robot's base coordinate system. , the following formula is used to perform sliding window filtering to obtain the denoised data : ； in Indicates the x-axis position of the human hand at time t in the base coordinate system of the robot arm, represents the time point of collecting the hand position, and k represents half of the filter window size; Then based on the same calculation formula, substitute or To replace Filter to obtain denoised data and ,in Indicates the y-axis position of the human hand at time t in the base coordinate system of the robot arm, represents the z-axis position of the human hand at time t in the base coordinate system of the robot arm, and is expressed as To represent the position of the human hand at time t in the robot base coordinate system, that is, the observation data point.

4. The method according to claim 3, characterized in that In the step 1, after the denoising step, the step of processing outliers is also included: Use the median absolute deviation to identify and correct outliers: first calculate the median of the three coordinate values ​​of each observation data point of the collected human motion posture, then calculate the absolute deviation of each observation data point from the median, and find the median of the absolute deviation : ； Where median(o) represents the median of the data set, and median() represents the function used to calculate the median; then outlier determination is performed: when the deviation of a data point exceeds three times When the value of is greater than , the data point is considered to be an outlier and is replaced by the average of the two adjacent non-outlier points.

5. The method according to claim 3, characterized in that: In the step 2, modeling the motion posture of the human hand based on the hidden Markov model includes: Modeling of the motion posture of the human hand based on the hidden Markov model using triples : ； in represents the initial state probability distribution, , It means that when the implicit variable The value is The probability of , represents the set of all possible values ​​of the implicit variable, , Represents the total number of all possible values ​​of the hidden variable, that is, the possible number of hidden states; represents the state transfer matrix, The elements in are , ,in represents the probability of transitioning from the state at the previous moment to the state at the current moment, The r and s in it represent the rows and columns of the state transfer matrix respectively. For in time The implicit variable of , represents a sequence of states, , T is the given time length; represents the observation state matrix, The elements in are , , Indicated in Observation status In the implicit variable The probability of generation under the condition, v represents the possible value of the observed variable, v V, V represents the set of all possible values ​​of the observed variable, , Represents the number of possible observation states.

6. The method according to claim 5, characterized in that State sequence Each state variable in Corresponding to the observation sequence An observed variable in , the observation sequence , each observation in the sequence That is, in time Observation data points ; and any implicit variable , any observed state .

7. The method according to claim 6, characterized in that After establishing the HMM, the collected observation data points are also used as the observation sequence Input into HMM, train HMM, and thus obtain HMM parameter information.

8. The method according to claim 7, characterized in that In step 2, predicting the future movement trajectory of the human hand includes: For The observed value obtained at the time , by obtaining To predict the observation value in the future, h represents the time interval; All state values ​​in are defined as a complete event group, and the following formula is obtained through the total probability formula: ； in Defined by the observation state matrix; for ,Will The values ​​of all states in are defined as a complete event group, which is obtained by the total probability formula: ； in Obtained from the state transfer matrix; and , and = , and is represented by the state transfer matrix, It is represented by the elements in the previous t+h-2 state transfer matrices; thus calculating , that is, calculated from arrive The distribution of the obstacle C is set as the expected value of the distribution. obs The location.

9. The method according to claim 8, characterized in that In step 3, the robot arm path is to solve the minimum value of the optimization problem expressed by the following formula: ； in, Indicates that the robotic arm that can be fully planned is arrive The path function, is the initial position of the robot arm, is the target position to which the robot arm will eventually move; min represents the minimum value, is the collision-free area, Represents all sampled points on the path; Represents the set of path functions that meet the above constraints.

10. The method according to claim 9, characterized in that The improved fast exploration random tree algorithm is used to The solution includes: In the working space of the robot arm Initialize two trees in and ,in from Start iteration, from Start the iteration, and in each iteration Random sampling to obtain sampling points , find the tree The closest point is defined as , and then get , where the steering function refers to towards Direction is turned to determine the direction of the new node, and the step size refers to and The maximum distance between them; then connect and , when an obstacle C appears on the connecting line obs , it means a collision occurs, and the system returns to , then exclude the original Find the distance The nearest point is taken as the new , until no C appears on the connecting line obs After that, The node is added to the tree, and then the iteration is repeated until and Connected to each other.

Images