A robotic arm path replanning method based on human hand trajectory prediction

Abstract

The present invention discloses a robotic arm path replanning method based on human hand trajectory prediction, comprising: Step 1, collecting images of the movement trajectory of a human hand of a staff member collaborating with the robotic arm in a three-dimensional space, and identifying the movement pose of the human hand according to the images; Step 2, based on the movement pose of the human hand obtained in Step 1, modeling the behavior and trajectory of the human hand based on a hidden Markov model, and then predicting the future movement trajectory of the human hand through learning of historical data; Step 3, using an improved rapidly-exploring random tree algorithm to generate a robotic arm path for the robotic arm from the current position to the target position while avoiding the future movement trajectory of the human hand predicted in Step 2. The robotic arm path replanning method based on human hand prediction proposed by 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.

Description

Technical Field

[0001] The present invention relates to the field of robotic arm control, and particularly to a robotic arm path replanning method based on human 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 has become increasingly important. Refer to Figure 1 , the human-robot collaboration system has provided great help in improving human work efficiency in fields such as manufacturing, healthcare, 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 an efficient human-machine interaction system.

[0003] Currently, traditional human-machine interaction safety strategies mainly rely on fixed safety measures, such as safety fences and emergency stop buttons. These measures often limit the interaction potential between robotic arms and humans. As the complexity of the environment increases, traditional methods are increasingly unable to meet the requirements of efficient and flexible operations. For example, in a factory, a robotic arm needs to closely cooperate with human colleagues in a constantly changing environment. At this time, relying solely on physical isolation and simple emergency stop buttons is no longer sufficient to ensure safety. Further, existing technologies have many deficiencies in dealing with human trajectory prediction and robotic arm path planning in dynamic environments. Although technologies such as optical tracking, radar, and other forms of sensors can provide real-time data, these technologies usually cannot 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, the following technical problems exist: 1) Safety risks in dynamic environments. In traditional human-machine interaction environments, dynamic and unstructured work scenarios often lead to safety risks, especially when humans and robotic arms need to closely cooperate in the same space. Existing technologies lack effective prediction and real-time response mechanisms and cannot fully predict human actions and paths, thus increasing the risk of collisions. 2) Path planning efficiency. In complex operating environments, traditional path planning methods often cannot meet the requirements of rapid response, resulting in slow response and low efficiency when the robotic arm avoids obstacles or adjusts its path. 3) Flexibility and efficiency of human-robot collaboration. Due to the lack of effective prediction and adjustment mechanisms, existing human-machine interaction systems often cannot achieve high efficiency and high flexibility in work performance while ensuring safety. Summary of the Invention

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

[0006] To achieve the above technical purpose, the technical solution of the present invention is as follows:

[0007] A robotic arm path replanning method based on human hand trajectory prediction, comprising the following steps:

[0008] Step 1, collect images of the movement trajectory of the human hand of the collaborative worker of the robotic arm in three-dimensional space, and identify the movement pose of the human hand according to the images;

[0009] Step 2, based on the movement pose of the human hand obtained in Step 1, model the movement pose of the human hand based on the hidden Markov model, and then predict the future movement trajectory of the human hand through learning of historical data;

[0010] Step 3, use an improved rapidly-exploring random tree algorithm to generate a robotic arm path from the current position to the target position and simultaneously avoid the future movement trajectory of the human hand predicted in Step 2.

[0011] For the method described above, Step 1 includes:

[0012] Capture images of the human hand of the collaborative worker of the robotic arm at a fixed frequency based on a depth camera; then send the captured images to an extraction model for extracting the movement pose of the human hand, so as to obtain the movement pose of the human hand.

[0013] For the method described above, after the extraction model extracts the movement pose of the human hand in Step 1, it further includes a denoising step:

[0014] Adopt a moving average filtering method to perform denoising on the data of the x-axis in the base coordinate system of the robotic arm , and perform sliding window filtering through the following formula to obtain the denoised data :

[0015] ;

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

[0017] Then substitute based on the same calculation formula for or to replace Perform filtering to obtain the denoised data and , where represents the y-axis position of the human hand at time t in the base coordinate system of the robotic arm, represents the z-axis position of the human hand at time t in the base coordinate system of the robotic arm, and is used to represent the position of the human hand at time t in the base coordinate system of the robotic arm, i.e., the observed data point.

[0018] For the method described above, in step 1, after the denoising step, it further includes a step of processing outliers:

[0019] Use the median absolute deviation to identify and correct outliers: First, calculate the median of the three coordinate values of each observed data point of the collected human motion pose, then calculate the absolute deviation of each observed data point from the median, and find the median of the absolute deviations :

[0020] ;

[0021] where median(o) represents the median of this set of data, and median() represents the function used to calculate the median; then perform outlier determination: When the deviation of a certain data point exceeds three times the value, then this data point is considered an outlier and is replaced with the average of the two adjacent non-outlier points.

[0022] For the method described above, in step 2, modeling the motion pose of the human hand based on the hidden Markov model includes:

[0023] Model the motion pose of the human hand based on the hidden Markov model through a triple :

[0024] ;

[0025] where represents the initial state probability distribution, , represents the probability when the hidden variable takes the value , , represents the set of all possible values of the hidden variable, , represents the total number of all possible values of the hidden variable, that is, the possible number of hidden states; represents the state transition matrix, and the elements in are ​, , where represents the probability of transitioning from the state at the previous moment to the state at the current moment, r and s in respectively represent the row and column of the state transition matrix, is the hidden variable at time , represents the state sequence, ; represents the observation state matrix, the elements in are , represents the probability of generating the observation state at time under the hidden variable , v represents the possible values of the observation variable, v V, V represents the set of all possible values of the observation variable, , represents the number of possible types of the observation state.

[0026] In the method described above, each state variable in the state sequence corresponds to an observation variable in the observation sequence , the observation sequence , T is the given time length, and each in the observation sequence, that is, the observation data point at time ; and any hidden variable , any observation state .

[0027] In the method described above, after establishing the HMM, it further includes the step of inputting the set of collected observation data points as the observation sequence into the HMM and training the HMM to obtain the HMM parameter information.

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

[0029] For the observed value obtained at time , predicting the observed values for a future period of time by obtaining , h represents the time interval; defining all the state values in as a complete event group, and obtaining the following formula through the total probability formula:

[0030] ;

[0031] wherein is defined by the observation state matrix;

[0032] For , all the state values in are defined as a complete event group, and obtained through the total probability formula:

[0033] ;

[0034] wherein is obtained from the state transition matrix;

[0035] while ;

[0036] and = , and is represented by the state transition matrix, is represented by the elements in the previous t + h - 2 state transition matrices; thus, is calculated, that is, the distribution from to is calculated, and the expected value of the distribution is used as the prediction value, and the prediction value is set as the position where the obstacle C obs is located.

[0037] In the method described above, in step 3, the manipulator path is the minimum value of the optimization problem expressed by solving the following formula:

[0038] ;

[0039] wherein, represents the path function of the manipulator that can be completely planned from to , is the initial position of the manipulator, is the target position that the manipulator finally moves to; min represents the minimum value, is the collision-free area, represents all the sampled points on the path; represents the set of path functions that meet the above constraint conditions.

[0040] In the method described above, using the improved rapidly-exploring random tree algorithm to solve includes:

[0041] Initializing two trees in the workspace of the manipulator, and the two trees are respectively and , wherein starting from Start the iteration, From Start the iteration. In each iteration, randomly sample a point in to obtain a sampled point , find the point in the tree closest to and define it as , then obtain through the steering function and step size, where the steering function refers to steering in the direction of to determine the direction of the new node, and the step size refers to the maximum distance between ; then connect and . When an obstacle C obs appears on the connecting line, it means a collision occurs. At this time, retreat to , then exclude the original and then find the point closest to as the new , until C obs does not appear on the connecting line, then add to the nodes in the tree. After that, continuously repeat the iteration until and are connected to each other.

[0042] The technical effect of the present invention is that a hidden Markov model is adopted for modeling and learning the time series data of human actions, so as to be able to predict the future positions and actions of humans. This prediction enables the robotic arm to more intelligently adjust its action plan, avoid potential conflicts, and optimize the work process. In addition, the present invention also adopts an improved rapidly-exploring random tree (RRT) algorithm, namely RRT-Connect, for real-time path planning. This algorithm effectively shortens the time for calculating the path by constructing a tree structure that rapidly grows towards the target area, improving the efficiency of path planning. When combined with the prediction data provided by the hidden Markov model, RRT-Connect can optimize the motion trajectory of the robotic arm on the premise of ensuring safety, thereby reducing operation interruptions and improving operation efficiency. By combining these two technologies, the robotic arm path replanning method based on human hand prediction proposed by 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-robot collaboration environments, especially suitable for complex tasks and environments that require close cooperation between the robotic arm and humans. BRIEF DESCRIPTION OF THE DRAWINGS

[0043] Figure 1 is a schematic diagram of the working scenario of human-robot collaboration processed by the present invention.

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

[0045] Figure 3 It is the system schematic 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 the schematic diagram of the RRT-Connet algorithm model in the present invention.

[0048] Figure 6 It is the comparison chart of the results of the present invention and other traditional algorithms. Among them, (a) is the schematic diagram of the results of LazyRRT; (b) is the schematic diagram of the results of RRTextend; (c) is the schematic diagram of the results of POA-RRT-Connect, that is, the results of the present invention; (d) is the schematic diagram of the results of RRTStar. Specific implementation manners

[0049] The following further describes the embodiments of the present invention with reference to the accompanying drawings.

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

[0051] S1, Definition of human position information: In the joint space of the robotic arm, with the coordinate system of the base coordinate of the robotic arm as the reference coordinate system, the human hand position data is collected. Define the position of the human hand in the base coordinate system of the robotic arm as , where respectively represent the coordinate, coordinate, and coordinate of the human hand, represents the time point when the camera collects the human hand position.

[0052] S2, Collection of human position information: Use a depth camera to collect the human position information during movement at a fixed frequency, and transmit the collected information to an extraction model for extracting the human hand movement pose, and use the extraction model to obtain the position of the human body in the base coordinate system of the robotic arm. In this embodiment, the depth camera used is the Realsense camera of Intel Corporation based on the structured light solution, and depth cameras using other solutions such as Tof and binocular imaging can also be used according to needs. See 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 and used with 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] Among them, median(o) represents the median of this set of data, and median() represents the function used to calculate the median; then outlier determination is performed: when the deviation of a certain data point exceeds three times of the value, then this data point is considered an outlier and is replaced with the average of the two adjacent non-outlier points.

[0059] S4, see Figure 4 , define the observation sequence and the state sequence: within a given time length T, the recorded motion data of the human hand is defined as the observation sequence , denoted as . Among them, each is the observation data point at time , that is, the mentioned above. Correspondingly, define a state sequence , denoted as , among which each is the hidden variable at time , and each hidden variable corresponds to an observation variable.

[0060] S5, define the value sets of the state and the observation variable: Define the set of all possible values of the observation variable as , where , where represents the possible number of types of the observation state. Define the set of all possible values of the state variable as , where , where represents the possible number of types of the hidden state. Among them, any hidden state , any observation state .

[0061] S6, establish a hidden Markov model: The hidden Markov model can be defined by a triple: . Among them represents the initial state probability distribution, . represents that when the state variable takes the value of , the probability, , 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 types of the hidden state, and the values of the hidden variable are learned by the hidden Markov model. , , among which represents the probability of transitioning from the state at the previous moment to the state at the current moment, In this, r and s respectively represent the rows and columns of the state transition matrix, for the latent variable at time ; , represents the state sequence, ; represents the observation state matrix, the elements in are , , represents at time the observation state generated under the latent variable , v represents the possible values of the observation variable, v V, V represents the set of all possible values of the observation variable, , represents the possible number of types of the observation state.

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

[0063] S7, use the hidden Markov model for prediction: take the human hand sequence as the observation sequence, and predict the human hand trajectory in the future for a period of time by observing the historical human motion trajectory. For the given observation value at time , to infer its observation value in the future for a period of time, then first calculate , where h represents the time interval. Define all the state values in as a complete event group, and the following formula can be obtained through the total probability formula:

[0064] ;

[0065] where is defined by the observation state matrix. And for , define all the state values in as a complete event group, and the following can be obtained through the total probability formula:

[0066] ;

[0067] where is obtained from the state transition matrix. For , from the fact that the hidden Markov model needs to satisfy the homogeneous Markov property and the observation independence assumption, , for further derivation can be obtained:

[0068] ;

[0069] Among them According to the homogeneous Markov theorem, its value is equal to , and it can be represented by the state transition matrix. Then it can be represented by the elements in the previous t + h - 2 state transition matrices. When calculating , it means that the distribution from to is calculated. According to this distribution, the expected value of this distribution is used as a relatively accurate prediction value, and this prediction value is set as the position where the obstacle is located .

[0070] S8, see Figure 5 , define the workspace of the robotic arm: Define the space where the robotic arm works as space, where the collision-free area is defined as , and the space where the obstacle is located is the obtained in S7. Define the initial position of the robotic arm as , and the target position that the robotic arm finally needs to move to is defined as .

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

[0072] ;

[0073] S10, robotic arm path replanning. First, initialize two trees in the workspace of the robotic arm. The two trees are respectively and , Start iterating from , Start from Start iteration. In each iteration, randomly sample in space to obtain , find the point in the tree closest to the sampling point and define it as . Then, obtain through the steering function and step size, where the steering function refers to steering in the direction to determine the direction of the new node, and the step size refers to the maximum distance between . Then connect and . If there is no obstacle on the connection line between the two, it means there is no collision with the obstacle . Then add to the nodes in the tree. If there is an obstacle C obs on the connection line, it means a collision occurs. At this time, retreat to , then exclude the original and then find the point closest to as the new , until there is no C obs on the connection line. Then add to the nodes in the tree. Then continuously iterate this process until the two trees are connected. When the two trees are connected, it indicates that the robotic arm has found a collision-free trajectory from the starting point to the ending point.

[0074] Next, verify and analyze by giving some specific examples. Set the following experimental parameters:

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

[0076] 2) The initial position planned by the robotic arm is [150, 400, 400].

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

[0078] The results are shown in Figure 6and Table 1 below, where POA-RRT-Connect represents the method adopted in this embodiment. As can be seen from the figure, this method can quickly find a path and is applicable to high-dimensional complex environments. Although there may be a problem that the path planning may not be the shortest path, since the present invention aims to ensure as much as possible that the robotic arm does not collide with the human body, even if the planned path is slightly longer in practice, it can actually 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 extension strategy, improving the collision detection method, and introducing an intelligent sampling mechanism. However, the implementation of RRTextend is relatively complex and relies heavily on domain knowledge. RRT Star, while maintaining the efficient exploration ability of RRT, makes the generated path not only feasible but also gradually improves in path quality and approaches the optimum with the increase of iterations by introducing a path optimization mechanism. However, its computational cost is high and parameter adjustment is complex, resulting in an overall implementation that is still too complex. The LazyRRT algorithm is an improvement of the traditional RRT algorithm, aiming to reduce the number of collision detections and thus improve the overall efficiency of the algorithm. LazyRRT speeds up the path planning process by delaying collision detection and reducing unnecessary collision checks. However, its planning time is long, and there may even be a problem that no path can be planned.

[0079] Table 1 Comparison of Different Algorithms

[0080] 。

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; Step 3, using an improved fast exploration random tree algorithm, generating a robotic arm path for the robotic arm from the current position to the target position while avoiding the future movement trajectory of the human hand predicted in step 2; The step 1 comprises: The robot arm cooperates with the human hands of the staff to capture images at a fixed frequency based on the depth camera; Then the captured image is sent to an extraction model for extracting the motion posture of the human hand, so as to obtain the motion posture of the human hand; 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. For the data x(t) of the x-axis in the base coordinate system of the robot, the sliding window filter is performed using the following formula to obtain the denoised data Where x(t) represents the x-axis position of the human hand at time t in the base coordinate system of the robot arm, t represents the time point when the human hand position is collected, and k represents half of the filter window size; Then, based on the same calculation formula, y(t) or z(t) is substituted for x(t) for filtering to obtain the denoised data. and Where y(t) represents the y-axis position of the human hand at time t in the base coordinate system of the robot arm, z(t) represents the z-axis position of the human hand at time t in the base coordinate system of the robot arm, and o(t) = (x(t), y(t), z(t)) is used to represent the position of the human hand at time t in the base coordinate system of the robot arm, that is, the observation data point.

2. The method according to claim 1, 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 MAD of the absolute deviation o : MAD o =median(|o(t)-median(o)|); 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 the MAD o 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.

3. The method according to claim 1, characterized in that In the step 2, modeling the motion posture of the human hand based on the hidden Markov model includes: The motion posture of the human hand is represented by triples based on the modeling of the hidden Markov model HMM: HMM = (π, A, B); Where π represents the initial state probability distribution, π=P(i1=q j ), P(i1=q j ) means that when the implicit variable i1 takes the value q j The probability of j ∈Q, Q represents the set of all possible values ​​of the implicit variable, Q=q1,q2…q m , m represents the total number of all possible values ​​of the hidden variable, that is, the number of possible types of hidden states; A represents the state transfer matrix, and the elements in A are a r,s , a r,s =P(i t |i t-1 ), where a r,s represents the probability of transitioning from the state at the previous moment to the state at the current moment, a r,s The r and s in the equation represent the rows and columns of the state transfer matrix, respectively. t is the implicit variable at time t, i t ∈I, I represents the state sequence, I=i1,i2…i T , T is the given time length; B represents the observation state matrix, and the elements in B are b s , b s =P(o t =v|i t =q), b s represents the observed state o at time t t In the implicit variable i t The probability of being generated under the condition that v represents the possible value of the observed variable, v∈V, V represents the set of all possible values ​​of the observed variable, V=v1,v2…v n , n represents the number of possible observation states.

4. The method according to claim 3, characterized in that: Each state variable i in the state sequence I t Each corresponds to an observation variable o in the observation sequence O t , observation sequence O = o1, o2…o T , for each o in the observation sequence t That is, the observed data point o(t) at time t; and any implicit variable i t ∈Q, any observed state o t ∈V.

5. The method according to claim 4, characterized in that After the HMM is established, the step of inputting the collected observation data point set as the observation sequence O into the HMM, training the HMM, and thus obtaining the HMM parameter information is also included.

6. The method according to claim 5, characterized in that In step 2, predicting the future movement trajectory of the human hand includes: For the observation value o obtained at time t t , by finding P(o t+h |o t ) to predict the observation value in the future, h represents the time interval; i t+h All state values ​​in are defined as a complete event group, and the following formula is obtained through the total probability formula: Where P(o t+h |i t+h =q k ) is defined by the observation state matrix; For P(i t+h =q k |o t ), change i t+h-1 The values ​​of all states in are defined as a complete event group, which is obtained by the total probability formula: Where P(i t+h |i t+h-1 =q j ) is obtained from the state transfer matrix; where P(i t+h-1 = q j | o t ) = P(i t+h-1 | i1, i2…i t+h-2 )P(i1, i2…i t+h-2 ). And P(i t+h-1 |i1,i2…i t+h-2 )=P(i t+h-1 |i t+h-2 ), and is represented by the state transfer matrix, P(i1,i2...i t+h-2 ) is represented by the elements in the previous t+h-2 state transfer matrices; thus P(o t+h |o t ), that is, calculated from o t to t+h The distribution of the obstacle C is set as the expected value of the distribution. obs The location.

7. The method according to claim 6, 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: Among them, λ represents the robot arm that can be completely planned from C init to C goal The path function, C init is the initial position of the robot arm, C goal is the target position to which the robot arm will eventually move; min represents the minimum value, C free is the collision-free area, τ represents all sampled points on the path; ∑ represents the set of path functions that meet the constraints, and the constraints are λ(τ)∈C free And λ(0)=C init ,λ(1)=C goal .

8. The method according to claim 7, characterized in that The improved fast exploration random tree algorithm is used to min The solution includes: Initialize two trees in the robot's workspace C. The two trees are T a and T b , where T a From C init Start iteration, T b From C goal Start iteration, randomly sample in C to obtain sampling point C in each iteration rand , find the tree away from C rand The closest point is defined as C near , and then C is obtained by the steering function and step size new , where the steering function refers to C near Towards C rand The direction is turned to determine the direction of the new node, and the step length refers to C new With C near The maximum distance between them; then connect C near With C new , when an obstacle C appears on the connecting line obs When , it means a collision occurs, and then it returns to C rand , then exclude the original C near Then find the distance C rand The closest point is taken as the new C new , until no C appears on the connecting line obs Then, C new Add the node to the tree, and then repeat the iteration until T a and T b Connected to each other.

Images