A Path Planning Method for Serial Robotic Arms Based on Gaussian Process
By adopting a Gaussian process-based method in robotic arm path planning, the problem of difficulty in balancing efficiency, quality and stability of traditional methods is solved, and efficient and stable path planning is achieved.
Patent Information
- Application Number
- CN202210986644.7
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2022-08-17
- Publication Date
- 2025-05-16
- Estimated Expiration
- 2042-08-17
AI Technical Summary
The traditional path planning method of random point sampling is difficult to balance path planning efficiency, quality and stability.
Using a tandem robotic arm path planning method based on Gaussian process, a smooth random path is sampled based on a Gaussian process model, and a high-quality motion path is screened using an lazy collision detection method.
It realizes the efficiency, quality and stability of robotic arm path planning, is suitable for high-dimensional and low-dimensional motion planning problems, and simplifies the path planning process.
Smart Images
Figure CN115229797B_ABST
Abstract
Description
Technical Field
[0001] The present invention belongs to the field of robot motion planning, and in particular relates to a serial robot arm path planning method based on Gaussian process, which is a random path sampling method mainly used for the path planning of serial robot arms. Background Art
[0002] Path planning is one of the key technologies to improve the level of robot intelligence. Its task is to search for a collision-free path from the starting position (position and posture) to the target position for the robot according to a certain evaluation standard in an environment with obstacles. At present, there are mainly the following types of path planning methods: 1) graph search method; 2) artificial potential field method; 3) random sampling method; 4) artificial intelligence method; 5) optimal control method. Graph search methods are often used for low-dimensional path planning problems; artificial potential field method and optimal control method are suitable for robot path planning in simple task scenarios; artificial intelligence methods have low efficiency in solving irregular complex problems; random sampling methods have high planning efficiency because they do not need to establish explicit expressions of obstacles in the environment. They are also suitable for low-dimensional and high-dimensional path planning, as well as simple and complex planning environments. However, the path planning method based on traditional random point sampling has the problem of difficulty in balancing the efficiency, quality and stability of path planning. Summary of the invention
[0003] The present invention provides a serial robot arm path planning method based on Gaussian process, which aims to solve the problem that the traditional random point sampling path planning method is difficult to balance the path planning efficiency, quality and stability.
[0004] Technical solution:
[0005] A Gaussian process-based serial robot arm path planning method comprises the following steps:
[0006] Step 1: Sample the initial pose nodes x of the robot in the configuration space init and the target pose node x goal ;
[0007] Step 2: Based on the Gaussian process model at the initial pose node x init and the target pose node x goal Time sampling n p smooth random paths;
[0008] Step 3: Calculate n in step 2 p The cost of a smooth random path;
[0009] Step 4: In order of value from small to large, replace the n in step 2 p Sorting the smooth random paths to obtain a sorted random sampling path set
[0010] Step 5: Use lazy collision detection to randomly sample a set of paths from step 4 The random path with the lowest cost is selected as the collision-free path, and the collision-free path is the high-quality motion path of the robot arm;
[0011] Step 6: If step 5 cannot screen out a collision-free path, repeat steps 2 to 5 until a high-quality motion path of the robot arm is screened out.
[0012] Furthermore, in step 2, sample n p The steps of the method for smoothing random paths are as follows:
[0013] Step 2.1: Determine the mathematical characteristics of the Gaussian process model, which include the mean function μ(s) and the kernel function k(s, s′);
[0014] Step 2.2: According to the mathematical characteristics of the Gaussian process model in step 2.1, the Gaussian process model of the random path of the robot is obtained, and the random sampling path δ = {x(s), s∈S} of a certain dimension in the robot configuration space is regarded as a Gaussian process, x(s) represents the position of the robot system under the index s, and S is the system index set;
[0015] Step 2.3: Based on the Gaussian process model of the random path of the robot in step 2.2, the probability posterior distribution of the random sampling path can be derived;
[0016] Step 2.4: Covariance matrix of the posterior probability distribution Perform matrix singular value decomposition, obtain a Gaussian random path sampling model based on the matrix singular value decomposition, and perform Gaussian random path sampling in a certain dimension of the robot arm configuration space;
[0017] Step 2.5: Repeat steps 2.2 to 2.4, perform a Gaussian random path sampling in each dimension of the robot configuration space, and merge all sampled random paths into a complete robot random sampling path;
[0018] Step 2.6: Repeat step 2.5 and randomly sample n in the robot configuration space p A complete random sampling path of the robot arm.
[0019] Furthermore, the mean function μ(s) in step 2.1 is set to 0; the kernel function k(s,s′) in the Gaussian process model is:
[0020]
[0021] Where s and s′ are system state indices, and parameter σ fReflects s=s ′ The probability distribution characteristics of the path state points, parameter σ l Reflects the correlation between different path points, parameter σ f and σ l Together they determine the characteristics of the continuous-time trajectory based on the Gaussian process,
[0022] Furthermore, the Gaussian process model of the random path of the robot in step 2.2 is:
[0023]
[0024] In the formula, δ is the set of randomly sampled paths, is the Gaussian process model representation symbol, μ(s) is the mean function, k(s,s′) is the quadratic exponential kernel function, and s and s′ are both system state indices.
[0025] Furthermore, the probability posterior distribution of the randomly sampled path in step 2.3 is:
[0026]
[0027] In the formula, μ p and Σ p are the mean vector and covariance matrix respectively; δ t ={x init ,x goal} and S t ={s1,s2} are the initial state of the robot, the target state set and the corresponding state index set; δ p ={x i |i=1,2,…,N} and S p ={s i |i=1,2,…,N} are the set of random path state points and the corresponding state index set respectively; the noise variance is given by Indicates that it is set to 0 during sampling; It's about S p and S t The kernel matrix of It's about S p Its own core matrix; It's about S t Its own kernel matrix; I is the identity matrix; x init and x goal are the initial state and target state of the robot, s1, s2 are the corresponding state indexes; δ p ={x i |i=1,2,…,N}, 1,2,…,N are the numbers of random path state points, x iis the state point in the random path. Furthermore, the covariance matrix in step 2.4 is Perform matrix singular value decomposition:
[0028]
[0029] Among them, U and V are unitary matrices, and ∑ is a diagonal matrix.
[0030] Furthermore, in step 2.4, the Gaussian random path sampling model is:
[0031]
[0032] in, is a Gaussian random component.
[0033] Furthermore, the lazy collision detection method in step 5 is:
[0034] Step 5.1: Select a random sampling path set The first random path with the smallest cost value is used to perform collision detection and boundary validity detection on sparse path points;
[0035] Step 5.2: If a path point collides with the surrounding environment or exceeds the boundary of the configuration space, then Delete the random path and repeat step 5.1;
[0036] Step 5.3: If the path point collision detection and boundary validity detection pass, Gaussian dense interpolation is performed between the sparse path points, and collision detection is performed on the dense interpolation points;
[0037] Step 5.4: If there is a collision between dense interpolation points, then in the random sampling path set Delete the random path and repeat steps 5.1 to 5.3;
[0038] Step 5.5: If the collision detection of the dense interpolation points passes, the path is a set of randomly sampled paths. The collision-free path with the smallest cost is the high-quality motion path of the robot arm.
[0039] Beneficial effects of the present invention:
[0040] The present invention proposes a path planning method for a serial robot arm based on a Gaussian process. This method is different from the traditional random point sampling motion planning method. It adopts a Gaussian process model to sample smooth random paths in the planning space, and uses an inert collision detection method and a high-quality path screening method to quickly output the motion path of a multi-degree-of-freedom robot arm. Finally, Gaussian interpolation is used to output dense path points for use by the robot arm's motion controller. The present invention has the characteristics of high efficiency, high planning quality, and strong stability in robot arm path planning. In addition, this method is not only applicable to both high-dimensional and low-dimensional motion planning problems, but also its simple path planning process is easier for engineering and technical personnel to understand and implement, and has practical and promotional value. BRIEF DESCRIPTION OF THE DRAWINGS
[0041] Figure 1 This is a flow chart of a serial robot arm path planning method based on Gaussian process in an embodiment of the present invention;
[0042] Figure 2 This is a schematic diagram of high-quality path screening in an embodiment of the present invention;
[0043] Figure 3 This is an experimental diagram of the path planning method proposed in an embodiment of the present invention. DETAILED DESCRIPTION
[0044] In order to make the purpose, technical solution and advantages of the present invention clearer, the present invention is further described in detail below in conjunction with the accompanying drawings and specific embodiments. The specific embodiments described here are only used to explain the present invention and are not used to limit the present invention.
[0045] A six-DOF serial robot path planning method based on Gaussian process, the process is as follows Figure 1 As shown, the following steps are included:
[0046] Step 1: Sample the initial pose nodes x of the six-DOF serial manipulator in the configuration space init and the target pose node x goal .
[0047] Step 2: Based on the Gaussian process model at node x init and x goal Time sampling n p The specific steps are as follows:
[0048] Step 2.1: Determine the mathematical characteristics of the Gaussian process model, which include the mean function μ(s) and the kernel function k(s, s′);
[0049] The mean function μ(s) and the kernel function k(s,s′) determine the mathematical characteristics of the Gaussian process model, where s is the system state index. If there is no special requirement, the mean function μ(s) is set to 0. To ensure the smoothness of the random sampling path, the kernel function in the Gaussian process model uses a quadratic exponential kernel function:
[0050]
[0051] Among them, s and s′ only represent the two input values of the kernel function k(s,s′), which are two system state indices. s and s′ may be the same or different, depending on the specific situation. The parameter σ f It reflects the probability distribution characteristics of the path state point when s = s′, and σ can be set f is 0.9 times the single-dimensional sampling range, and the parameter σ l Reflect the correlation between different path points, let σ l It is set to 0.8 to ensure that the sampling path has a certain volatility.
[0052] Step 2.2: According to the mathematical characteristics of the Gaussian process model in step 2.1, a random path model of the robot arm described by the Gaussian process is obtained;
[0053] The random sampling path δ = {x(s), s∈S} of a certain dimension in the robot configuration space is regarded as a Gaussian process. The dimension mentioned in this application refers to the degree of freedom of the robot, x(s) represents the position of the robot system under the index s, and S is the system index set. Then the random path model of the robot can be described as:
[0054]
[0055] Step 2.3: Based on step 2.2, the probability posterior distribution of the random sampling path can be derived:
[0056]
[0057] Among them, μ p and Σ p are the mean vector and covariance matrix respectively; δ t ={x init ,x goal} and S t ={s1,s2} are the initial state of the robot, the target state set and the corresponding state index set; δ p ={x i |i=1,2,…,N} and S p ={s i |i=1,2,…,N} are the set of random path state points and the corresponding state index set respectively; the noise variance is given by Indicates that it is set to 0 during sampling; It's about S p and S t The kernel matrix of It's about S p Its own core matrix; It's about S t Its own kernel matrix; k pt , k p , K t It can be calculated by formula (1).
[0058] Step 2.4: Covariance matrix of the probability posterior distribution Perform matrix singular value decomposition, and then use the Gaussian random path sampling model to perform Gaussian random path δ in a certain dimension of the robot configuration space based on the matrix singular value decomposition. p ={x i |i=1,2,…,N} sampling;
[0059] For the covariance matrix in formula (3), Perform matrix singular value decomposition:
[0060]
[0061] Among them, U and V are unitary matrices, and ∑ is a diagonal matrix. Then, using formula (5), a Gaussian random path δ is generated in a certain dimension of the robot configuration space. p ={x i |i=1,2,…,10} sampling:
[0062]
[0063] in, is a Gaussian random component.
[0064] Step 2.5: Repeat steps 2.2 to 2.4, perform Gaussian random path sampling in the six dimensions of the robot configuration space, and merge all random sampling paths into a complete robot random sampling path.
[0065] Step 2.6: Repeat step 2.5 and randomly sample n in the robot configuration space p A complete random path.
[0066] Step 3: Calculate the cost of all random paths in step 2.
[0067] Step 4: Sort all random paths in step 2 in ascending order of cost value to obtain a sorted set of random sampling paths.
[0068] Step 5: Use the lazy collision detection method to select the collision-free path with the lowest cost from the random paths sorted in step 4, that is, the high-quality motion path of the robot arm, such as Figure 2 As shown, Figure 2 Schematic diagram of high-quality path screening in an embodiment of the present invention; wherein (a) is a random path based on sparse sampling points; (b) is a random path subjected to dense interpolation and collision detection in the order of cost values; and (c) is a collision-free smooth path with the smallest cost value selected.
[0069] The specific steps of the lazy collision detection method are as follows:
[0070] Step 5.1: Select a random sampling path set The first random path in the set, that is, the random path with the smallest cost in the set, is used to perform collision detection and boundary validity detection on sparse path points.
[0071] Step 5.2: If a path point collides with the surrounding environment or exceeds the boundary of the configuration space, then Delete the random path and repeat step 5.1.
[0072] Step 5.3: If the path point collision detection and boundary validity detection pass, Gaussian dense interpolation is performed between the sparse path points, and collision detection is performed on the dense interpolation points.
[0073] Step 5.4: If there is a collision between dense interpolation points, then in the random sampling path set Delete the random path and repeat steps 5.1 to 5.3.
[0074] Step 5.5: If the collision detection of the dense interpolation points passes, the path is a set of randomly sampled paths. The collision-free path with the smallest cost is the high-quality motion path of the robot arm.
[0075] Step 6: If step 5 cannot screen out a collision-free path, repeat steps 2 to 5 until a high-quality motion path of the robot arm is screened out.
[0076] The control method provided by the present invention is experimentally verified, and the experimental environment is as follows:
[0077] The experimental platform is a humanoid service robot equipped with a six-degree-of-freedom robotic arm. Its main control configuration is an industrial computer with an Intel Core i7-3610QE 2.3GHz CPU and 8GB of memory. The system is the open source robot operating system ROS.
[0078] In this embodiment, the path planning method proposed in the present invention is used to perform obstacle avoidance path planning for a six-degree-of-freedom serial robot arm, such as Figure 3As shown. Set each random path to consist of 10 nodes and the number of random path samples n p =1000, the end effector position error is 10 -3 meters, the attitude error is 10 -3 The experimental results are shown in Table 1, where the comparison methods are the Rapidly Expanding Random Tree Method RRT and the Fast Forward Tree Method FMT*, and the Gaussian Random Path Motion Planning Method proposed in the present invention is represented by GRPMP.
[0079] Table 1 Path planning experimental results
[0080]
[0081] It can be seen from the experimental results that the Gaussian process-based serial robot path planning method of the present invention can meet the requirements of fast and high-quality path planning of multi-degree-of-freedom robot arms, and has high application value and scalability.
[0082] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit it. Although the present invention has been described in detail with reference to the aforementioned embodiments, it should be understood by those skilled in the art that the technical solutions described in the aforementioned embodiments can still be modified, or some or all of the technical features therein can be replaced by equivalents. Therefore, these modifications or replacements do not cause the essence of the corresponding technical solutions to deviate from the scope defined by the claims of the present invention.
Claims
1. A serial robot arm path planning method based on Gaussian process, characterized in that: The following steps are involved: Step 1: Sample the initial pose nodes x of the robot in the configuration space init and the target pose node x goal ; Step 2: Based on the Gaussian process model at the initial pose node x init and the target pose node x goal Time sampling n p smooth random paths; Step 3: Calculate n in step 2 p The cost of a smooth random path; Step 4: In order of value from small to large, replace the n in step 2 p Sorting the smooth random paths to obtain a sorted random sampling path set Step 5: Use lazy collision detection to randomly sample a set of paths from step 4 The random path with the lowest cost is selected as the collision-free path, and the collision-free path is the high-quality motion path of the robot arm; Step 6: If step 5 fails to screen out a collision-free path, repeat steps 2 to 5 until a high-quality motion path of the robot arm is screened out; In step 2, sample n p The steps of the method for smoothing random paths are as follows: Step 2.1: Determine the mathematical characteristics of the Gaussian process model, which include the mean function μ(s) and the kernel function k(s, s′); Step 2.2: According to the mathematical characteristics of the Gaussian process model in step 2.1, the Gaussian process model of the random path of the robot is obtained, and the random sampling path δ = {x(s), s∈S} of a certain dimension in the robot configuration space is regarded as a Gaussian process, x(s) represents the position of the robot system under the index s, and S is the system index set; Step 2.3: Based on the Gaussian process model of the random path of the robot in step 2.2, the probability posterior distribution of the random sampling path can be derived; Step 2.4: Covariance matrix of the posterior probability distribution Perform matrix singular value decomposition, obtain a Gaussian random path sampling model based on the matrix singular value decomposition, and perform Gaussian random path sampling in a certain dimension of the robot arm configuration space; Step 2.5: Repeat steps 2.2 to 2.4, perform a Gaussian random path sampling in each dimension of the robot configuration space, and merge all sampled random paths into a complete robot random sampling path; Step 2.6: Repeat step 2.5 and randomly sample n in the robot configuration space p A complete random sampling path of the robot arm; In step 2.1, the mean function μ(s) is set to 0; the kernel function k(s, s′) in the Gaussian process model is: In the formula, s and s ′ is the system state index, parameter σ f Reflects s=s ′ The probability distribution characteristics of the path state points, parameter σ l Reflects the correlation between different path points, parameter σ f and σ l Jointly determine the continuous-time trajectory characteristics based on Gaussian processes; The Gaussian process model of the random path of the robot in step 2.2 is: In the formula, δ is the set of randomly sampled paths, is the symbol for the Gaussian process model, μ(s) is the mean function, k(s,s ′ ) is a quadratic exponential kernel function, s and s' are both system state indexes; The probability posterior distribution of the randomly sampled path in step 2.3 is: In the formula, μ p and Σ p are the mean vector and covariance matrix respectively; δ t ={x init ,x goal } and S t ={s1,s a } are the initial state of the robot, the target state set and the corresponding state index set; δ p ={x i |i=1,2,…,N} and S p ={s i |i=1,2,…,N} are the set of random path state points and the corresponding state index set respectively; the noise variance is given by Indicates that it is set to 0 during sampling; It's about S p and S t The kernel matrix of It's about S p Its own core matrix; It's about S t Its own kernel matrix; I is the unit matrix; x init and x goal are the initial state and target state of the robot, s1, s2 are the corresponding state indexes; δ p ={x i |i=1,2,…,N}, 1,2,…,N are the numbers of random path state points, x i is the state point in the random path; The covariance matrix in step 2.4 Perform matrix singular value decomposition: Among them, U and V are unitary matrices, and ∑ is a diagonal matrix; In step 2.4, the Gaussian random path sampling model is: in, is a Gaussian random component.
2. The Gaussian process-based serial robot path planning method according to claim 1, characterized in that: The lazy collision detection method in step 5 is: Step 5.1: Select a random sampling path set The first random path with the smallest cost value is used to perform collision detection and boundary validity detection on sparse path points; Step 5.2: If a path point collides with the surrounding environment or exceeds the boundary of the configuration space, then Delete the random path and repeat step 5.1; Step 5.3: If the path point collision detection and boundary validity detection pass, Gaussian dense interpolation is performed between the sparse path points, and collision detection is performed on the dense interpolation points; Step 5.4: If there is a collision between dense interpolation points, then in the random sampling path set Delete the random path and repeat steps 5.1 to 5.3; Step 5.5: If the collision detection of the dense interpolation points passes, the path is a set of randomly sampled paths. The collision-free path with the smallest cost is the high-quality motion path of the robot arm.