Metaheuristic mechanical arm path rapid searching and obstacle avoiding method

Abstract

The invention belongs to the field of artificial intelligence, discloses a meta-heuristic mechanical arm path quick search obstacle avoidance method, and aims to solve the problems of slow convergence, long planning time and the like caused by a random sampling strategy of an existing mechanical arm obstacle avoidance algorithm. The hybrid sampling strategy is implemented by fusing the improved Gaussian hybrid distribution sampling strategy and the map complexity constraint strategy. According to the method, sampling random points in a barrier-free area are reduced, sampling random points close to a barrier area are increased, so that the generation speed and sampling efficiency of a planned path are improved, the whole-course controllability of a joint space is needed in the barrier-avoiding searching process, the physical limits of joint control position quantity and speed quantity are not broken through, and the operation safety of the mechanical arm is guaranteed.

Description

Technical Field

[0001] The invention belongs to the field of artificial intelligence, and in particular relates to a meta-heuristic robot arm path rapid search and obstacle avoidance method. Background Art

[0002] The traditional robot path obstacle avoidance algorithm adopts the global area, and the exploration area is completely random. When the target offset point is blocked by an obstacle, the global area is still randomly sampled, which lacks target orientation. This makes the traditional obstacle avoidance search method have the following defects:

[0003] 1. It is impossible to select the sampling point closest to the target point, perform invalid obstacle avoidance exploration in unnecessary space, and generate invalid obstacle avoidance search directions;

[0004] 2. Traditional obstacle avoidance search uses fixed step size expansion, cannot adaptively adjust the search speed, and has slow iterative update speed;

[0005] 3. There is a lack of angle restriction constraint strategy, and the angle between the line connecting the current sampling point and the current point and the line connecting the starting point and the end point is not used to limit the path search, which can easily cause the robot arm to overshoot and break the physical limit of the joint.

[0006] Therefore, the target obstacle sampling points are concentrated near the center of the target point, and the path sampling points have the best effect and quality. When the target obstacle sampling points are far away from the center of the target, the effect of the path sampling points is reduced. Therefore, how to ensure that the search direction of the robot arm quickly approaches the center area of ​​the target point is crucial to the efficiency of obstacle avoidance. It is necessary to avoid invalid and irrelevant space searches and select the optimal search direction; it is also necessary to appropriately accelerate the search speed and use a variable step search method to adjust the obstacle avoidance search speed; at the same time, the joint space needs to be fully controllable during the obstacle avoidance search process, and the physical limits of the joint control position and speed cannot be exceeded to ensure the safe operation of the robot arm. Summary of the invention

[0007] The present invention provides a meta-heuristic robot path fast search obstacle avoidance method to solve the problems of slow convergence and long planning time caused by the random sampling strategy of the existing robot obstacle avoidance algorithm. The present invention is based on the improved Gaussian mixture distribution sampling strategy to perform interval constraints on the generation of random trees, reduce the sampling random points in the obstacle-free area, and increase the sampling random points near the obstacle area, thereby improving the generation speed of the planned path.

[0008] In order to solve the technical problem, the technical solution adopted by the present invention is:

[0009] A meta-heuristic method for rapid search and obstacle avoidance of a robot path, characterized by comprising:

[0010] (1) During the path planning process, the robot arm uses an improved Gaussian mixture distribution sampling strategy to impose interval constraints on the generation of random trees to improve the generation speed of the planned path.

[0011] (2) The robot arm introduces a map complexity assessment strategy during the sampling process to reduce the time spent on finding the optimal path point;

[0012] (3) A hybrid sampling strategy is implemented by integrating the improved Gaussian mixture distribution sampling strategy and the map complexity constraint strategy.

[0013] In some embodiments, in the improved Gaussian mixture distribution sampling strategy, the n-dimensional function expression of the Gaussian function is as follows:

[0014]

[0015] σ is the standard deviation of the Gaussian distribution function, representing the distance between the sampling point and the obstacle, and μ is the expected value.

[0016] The probability distribution function of the sampling point in the obstacle area is:

[0017] φ(x;μ,σ)=∫f(x;μ,σ)dx; (2)

[0018] From the above probability distribution function, we can see that the closer to the obstacle, the greater the sampling probability. In order to avoid collision between the sampling point and the obstacle, the obstacle function obs(x) is set and the probability distribution function is improved. The improved probability distribution function expression is as follows:

[0019] g(x;μ,σ)=max(0,φ(x;μ,σ)-obs(x)); (3)

[0020] Where obs(x) is the obstacle function. When the generated sampling point collides with an obstacle, obs(x) = 1, otherwise it is 0. If the sampling point falls in the obstacle-free area, g(x; μ, σ) = φ(x; μ, σ), otherwise g(x; μ, σ) = 0.

[0021] In some embodiments, the robot arm introduces a map complexity evaluation strategy during the sampling process to reduce the time for finding the optimal path point; at the same time, a constraint threshold p is set.

[0022] Among them, the map complexity coefficient C 1 It is expressed as:

[0023] 0.5 represents the weight ratio, S obc Represents the obstacle area, S map Indicates the map area, D obcIndicates the distribution of obstacles.

[0024] In some embodiments, the hybrid sampling strategy integrating the improved Gaussian mixture distribution sampling strategy and the map complexity constraint strategy is specifically implemented as follows:

[0025] (1) Set the constraint threshold p. If the probability of generating a sampling point is less than the constraint threshold p, a point Q is randomly generated. rand1 Added random trees;

[0026] (2) If Q rand1 If a collision occurs with an obstacle, a point Q is regenerated through the improved probability distribution function (i.e., formula (3) described above): rand2 ; If no collision occurs, return Q rand1 ;

[0027] (3) If Q rand2 If it collides with an obstacle, select Q rand1 and Q rand2 The midpoint Q in space rand3 If no collision occurs, return Q rand2 ;

[0028] (4) If Q rand3 If there is no collision with the obstacle, then Q rand3 is the sampling point of the random tree, otherwise, return to the first step and resample.

[0029] In some embodiments, the robot arm introduces a sampling constraint strategy during path planning based on a hybrid sampling strategy. The sampling constraint strategy specifically includes:

[0030] The angle θ between the line connecting the previous sampling point and the current point and the line connecting the starting point and the end point is used as the criterion for judging the positive growth of the sampling point; when the angle θ∈[-π / 2,π / 2], it is judged that the random sampling point belongs to positive growth; when the angle θ is not in the range of [-π / 2,π / 2], the random sampling point does not meet the sampling constraint requirements (i.e., it belongs to reverse growth), and the random sampling point should be eliminated.

[0031] In some embodiments, the present invention introduces an adaptive step size strategy in the sampling process to generate a new node Q new , reducing the probability of collision between the new extended node and obstacles. New node Q new Determined by the following formula:

[0032]

[0033] Among them, ρ 1 It's Q new Step size coefficient towards random sampling point, ρ 2It's Q new The step length coefficient towards the target point. 1 >ρ 2 When the new node Q new Grow towards the position and direction of the random sampling point; when ρ 1 <ρ 2 , new node Q new Grow towards the target point position and direction.

[0034] Therefore, in some embodiments, the robot adopts a hybrid sampling strategy and a sampling constraint strategy during the path planning process, and adopts an adaptive step size strategy to generate a new node Q new , to reduce the probability of collision between the newly extended node and obstacles.

[0035] In some embodiments, the robot generates a reference trajectory x during the path planning process. r (t), when the robot moves along the reference trajectory x r (t) When in motion, the following relationship should be satisfied:

[0036] x t (t) = f(θ r (t)); (6)

[0037] Among them, θ r (t) is the reference trajectory x r (t) Corresponding joint motion trajectory.

[0038] During the actual operation of the robot arm, its joint angles are subject to certain restrictions. That is, the generated joint angles should not exceed their physical limits, and corresponding constraints should be set:

[0039]

[0040] Among them, g(x r (t),θ(t)) represents the objective function, m represents the number of robot joints, Indicates the minimum and maximum limit angles of the kth joint of the robot. At this time, a tracking error e is defined, and its value is e = x r (t)-f(θ(t)), and put it into formula (7), so that formula (7) is transformed into:

[0041]

[0042] stθ - <θ<θ + ; (8)

[0043] In some embodiments, after the joint of the robotic arm moves to a position, the direction vector, antenna coordinates and objective function value are updated, and the process is repeated until the path planning is completed.

[0044] Compared with the prior art, the present invention has the following beneficial effects:

[0045] The meta-heuristic robot path fast search and obstacle avoidance method of the present invention adopts Gaussian mixture distribution and dynamic target bias probability strategy to sample random points and guide the random tree to expand to the target point.

[0046] The sampling point closest to the target point is selected through multiple sampling, which reduces the algorithm's exploration of unnecessary space and avoids the expansion and generation of invalid directions.

[0047] The variable step size expansion method allows the search tree to quickly find paths with fewer nodes and iterations.

[0048] An angle restriction constraint strategy is introduced. When the angle between the line connecting the previous sampling point and the current point is equal to the angle between the line connecting the starting point and the end point, the current point is used as a new sampling point and added to the random tree.

[0049] In summary, the present invention can improve the generation speed and sampling efficiency of the planned path, and the obstacle avoidance search process requires that the joint space is fully controllable without breaking the physical limits of the joint control position and speed, thereby ensuring the safe operation of the robotic arm. BRIEF DESCRIPTION OF THE DRAWINGS

[0050] Figure 1 This is a schematic diagram of the equivalent interactive detection between the robot arm and the rectangular obstacle; Figure 1 It includes 1(a) and 1(b). In 1(a), the line segment and the plane are parallel to each other, and in 1(b), the line segment and the plane intersect.

[0051] Figure 2 This is a schematic diagram of the equivalent interactive detection between the robot arm and the cylindrical obstacle;

[0052] Figure 3 Schematic diagram of equivalent interactive detection between the robot arm and the spherical obstacle;

[0053] Figure 4 Schematic diagram of the sampling constraint strategy of the present invention. DETAILED DESCRIPTION

[0054] The present invention is further described below in conjunction with embodiments, which are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, other embodiments obtained by ordinary technicians in the field without creative work are all within the scope of protection of the present invention.

[0055] In the description of the present invention, it should be noted that the terms "center", "upper", "lower", "left", "right", "vertical", "horizontal", "inside", "outside" and the like indicate positions or positional relationships based on the positions or positional relationships shown in the drawings, and are only for the convenience of describing the present invention and simplifying the description, rather than indicating or implying that the device or element referred to must have a specific orientation, be constructed and operated in a specific orientation, and therefore cannot be understood as limiting the present invention; the terms "first", "second", and "third" are only used for descriptive purposes, and cannot be understood as indicating or implying relative importance; in addition, unless otherwise clearly specified and limited, the terms "installed", "connected", and "connected" should be understood in a broad sense, for example, it can be a fixed connection, a detachable connection, or an integral connection; it can be directly connected or indirectly connected through an intermediate medium. For those of ordinary skill in the art, the specific meanings of the above terms in the present invention can be understood according to specific circumstances.

[0056] The present invention takes into account the computational efficiency of the robot algorithm's obstacle avoidance and collision detection, and adopts the idea of ​​bounding boxes and space superposition. The arm base is modeled as a rectangular bounding box. The robot, arm support structure, operating space, positioning device, etc. are modeled as a cylindrical bounding box. The connecting device, anchor positioning clip, double-ear sleeve, etc. are modeled as a spherical bounding box. The robot's connecting rod maximizes the bounding box as an obstacle. At this time, the collision detection between the robot and the obstacle is converted into an intersection detection between the bounding box line segment and the cuboid, cylinder, and sphere. Although the above strategy will waste part of the robot's working space, it greatly simplifies the calculation process of collision detection and improves the efficiency and reliability of planning.

[0057] like Figure 1 As shown in the figure, when the robot arm interacts with the rectangular obstacle, there are two different positional relationships, namely, the line segment is parallel to a plane and the line segment intersects a plane. The distance between the robot arm link and the obstacle is determined by the following two formulas.

[0058]

[0059] In the formula Medium, d 1 is the distance between the line segment and the cuboid when they are parallel, M is the coordinate of the starting point of the line segment (x 1 ,y 1 ,z 1 ), P is the coordinate of the center point of the parallel surface of the cuboid (x 0 ,y 0 ,z 0 ), is the direction vector of the line segment (m,n,p). 1 Maximize the bounding box radius r with the robot link 1When the sum is greater than 0, the robot arm link does not collide with the obstacle.

[0060] In the formula In the equation, G is the intersection point of the line segment and a plane (Gx Gy Gz), and A, B, C, and D are the coefficients of the plane equation of the intersection of the line segment and the cuboid, Ax+By+Cz+D=0. This value is determined by the coordinates of the three vertices of the cuboid plane.

[0061] When (G.x+r 1 )>x max ||(G.y+r 1 )>y max ||(G.z+r 1 )>z max When the line segment intersects a plane, the robot arm link does not collide with the obstacle.

[0062] like Figure 2 As shown in , when the robot arm interacts with the cylindrical obstacle, it is transformed into the interaction detection between the equivalent line segment of the robot arm link and the equivalent line segment of the cylindrical obstacle. Figure 2 It can be seen that if we take a point E on the line segment MN and slide it back and forth on MN, the coordinates of point E are:

[0063]

[0064] Where a is the sliding parameter. When 0≤a≤1, E is on MN. When 1<a, E is on When a<0, E is on the extension line of Similarly, take a point F on the line segment UV and slide it back and forth on UV. The coordinates of point F are:

[0065]

[0066] Where b is the sliding parameter. When 0≤b≤1, F is on UV. When 1<b, F is on When b<0, F is on the extension line of At this time, the distance d between the line segments 2 for:

[0067]

[0068] Substitute the coordinates of point E and point F into the above equation and solve for d 2 The minimum value of d 2min Maximize bounding box radius r with the link 1 The maximum bounding box distance r from the obstacle 2 When d 2min >(r 1+r 2 ), the robot arm connecting rod will not collide with obstacles.

[0069] like Figure 3 As shown in Figure 1, when the robot arm interacts with the spherical obstacle, it can be transformed into an interaction detection between the sphere center point O and the equivalent line segment MN of the robot arm link. Figure 3 As shown, the shortest distance between the center point O and the equivalent line segment MN is:

[0070]

[0071] Where O is the center of the spherical obstacle and MN is the equivalent connecting rod. Figure 3 , define the sphere radius as the sphere obstacle radius R and the connecting rod radius r 1 The sum of d 3 >R+r 1 When the robot arm is in motion, the robot arm link will not collide with obstacles.

[0072] Based on the above strategies, the meta-heuristic robot path fast search and obstacle avoidance method of the present invention includes:

[0073] (1) During the path planning process, the robot arm uses an improved Gaussian mixture distribution sampling strategy to impose interval constraints on the generation of random trees to improve the generation speed of the planned path.

[0074] (2) The robot arm introduces a map complexity assessment strategy during the sampling process to reduce the time spent on finding the optimal path point;

[0075] (3) A hybrid sampling strategy is implemented by integrating the improved Gaussian mixture distribution sampling strategy and the map complexity constraint strategy.

[0076] In the improved Gaussian mixture distribution sampling strategy, the n-dimensional function expression of the Gaussian function is as follows:

[0077]

[0078] σ is the standard deviation of the Gaussian distribution function, representing the distance between the sampling point and the obstacle, and μ is the expected value.

[0079] The probability distribution function of the sampling point in the obstacle area is:

[0080] φ(x;μ,σ)=∫f(x;μ,σ)dx; (2)

[0081] From the above probability distribution function, we can see that the closer to the obstacle, the greater the sampling probability. In order to avoid collision between the sampling point and the obstacle, the obstacle function obs(x) is set and the probability distribution function is improved; the improved probability distribution function expression is as follows:

[0082] g(x;μ,σ)=max(0,φ(x;μ,σ)-obs(x)); (3)

[0083] Among them, obs(x) is the obstacle function set. When the generated sampling point collides with the obstacle, obs(x) is defined as 1, otherwise it is 0. If the sampling point falls in the obstacle-free area, g(x; μ, σ) = φ(x; μ, σ), otherwise g(x; μ, σ) = 0. In other words, when the collected points do not meet the path planning requirements, resampling is required to guide the search for suitable sampling points.

[0084] Based on the improved Gaussian mixture distribution sampling strategy, interval constraints are imposed on the generation of random trees, which reduces the sampling random points in the obstacle-free area and increases the sampling random points in the area close to obstacles, thereby improving the generation speed of the planned path.

[0085] However, in the case of a large number of obstacles or complex environments, random points on the search tree (i.e., sampling random points) will fall into the local minimum, resulting in more time spent on finding the optimal path point. In view of this situation, a map complexity evaluation strategy is introduced to calculate the most appropriate map complexity coefficient C 1 , get the most suitable constraint threshold p.

[0086] In some embodiments, the robotic arm introduces a map complexity assessment strategy during the sampling process to reduce the time spent searching for optimal path points.

[0087] Map complexity coefficient C 1 It is expressed as:

[0088] 0.5 represents the weight ratio, S obc Represents the obstacle area, S map Indicates the map area, D obc Indicates the distribution of obstacles. That is, the map is divided into 100 equal grids, and the ratio of the number of grids occupied by obstacles to 100 is the distribution of obstacles. 1 The closer it is to 1, the more complex the map is, and the smaller the constraint threshold p is. According to calculations, the constraint threshold p is set to p = (1-C 1 ) 3 , and its path planning has the lowest time cost.

[0089] The specific implementation steps of the hybrid sampling strategy that integrates the improved Gaussian mixture distribution sampling strategy and the map complexity constraint strategy are as follows:

[0090] (1) Set the constraint threshold p. If the probability of generating a sampling point is less than the constraint threshold p, a point Q is randomly generated.rand1 Added random trees;

[0091] (2) If Q rand1 If a collision occurs with an obstacle, a point Q is regenerated through the improved probability distribution function (i.e., formula (3) described above): rand2 ; If no collision occurs, return Q rand1 ;

[0092] (3) If Q rand2 If it collides with an obstacle, select Q rand1 and Q rand2 The midpoint Q in space rand3 If no collision occurs, return Q rand2 ;

[0093] (4) If Q rand3 If there is no collision with the obstacle, then Q rand3 is the sampling point of the random tree, otherwise, return to the first step and resample.

[0094] Although the hybrid strategy of fusing the improved Gaussian mixture distribution sampling strategy and the map complexity constraint strategy can ensure that the sampling points are evenly distributed in the obstacle area, the generation of sampling points does not have directionality, and it is easy to have an inverse growth scene, which increases the time cost and path cost of path planning. In order to avoid this situation. The present invention proposes a sampling path strategy to constrain the sampling points of the hybrid sampling strategy.

[0095] Therefore, in some embodiments, the robot arm introduces a sampling constraint strategy in the process of path planning based on the hybrid sampling strategy, and the sampling constraint strategy specifically includes:

[0096] The angle θ between the line connecting the previous sampling point and the current point and the line connecting the starting point and the end point is used as the criterion for judging the positive growth of the sampling point; when the angle θ∈[-π / 2,π / 2], it is judged that the random sampling point belongs to positive growth; when the angle θ is not in the range of [-π / 2,π / 2], the random sampling point does not meet the sampling constraint requirements (i.e., it belongs to reverse growth), and the random sampling point should be eliminated.

[0097] Combined with Figure 4 , as attached Figure 4 As shown, Q init is the initial point, Q goal is the target point, θ 1 ,θ 2 ,θ 3 ,θ 4 ,θ 5 ,θ 6 It is the angle between the line connecting the previous sampling point and the current point and the line connecting the starting point and the end point.rand1 , Q rand2 , Q rand3 ,,Q rand4 , Q rand6 , Q rand7 Randomly generated by the constraint threshold strategy, Q rand5 Generated by the mixed Gaussian distribution function (i.e., generated by the previous probability distribution function formula (3)). When the sampling point is in the non-obstacle area, it can be directly used as an effective sampling node. When the sampling point is in the obstacle area, according to formula (3), the center point Q between the two obstacles is set rand5 As sampling point. Figure 4 It can be seen that when the sampling constraint strategy is used, θ 1 and θ 6 The value of is not in the range [-π / 2,π / 2], which corresponds to the random sampling point Q rand2 and Q rand7 It does not meet the sampling constraint requirements and should be eliminated.

[0098] In the specific implementation process, although the hybrid sampling strategy and sampling constraint strategy solve the interval point generation and forward expansion problems of the random tree, the expansion of the sampling points of the standard RRT algorithm still has a large degree of randomness, and the generation of new nodes is carried out along the direction of the sampling points, rather than always growing towards the target node. In addition, there is still a certain probability that the sampling points are generated near the obstacle area, which increases the probability of collision between the newly expanded nodes and obstacles, such as Figure 4 Q shown rand4 Therefore, based on the fixed step-size expansion strategy of the standard RRT algorithm, the present invention provides an adaptive step-size strategy to generate a new node Q new , reducing the probability of collision between the new extended node and obstacles. New node Q new Determined by the following formula:

[0099]

[0100] Among them, ρ 1 It's Q new Step size coefficient towards random sampling point, ρ 2 It's Q new The step length coefficient towards the target point. 1 >ρ 2 When the new node Q new Grow towards the position and direction of the random sampling point; when ρ 1 <ρ 2 , new node Q new Grow towards the target point position and direction.

[0101] In some embodiments, the robot adopts a hybrid sampling strategy and a sampling constraint strategy during the path planning process, and adopts an adaptive step size strategy to generate a new node Q new , to reduce the probability of collision between the newly extended node and obstacles.

[0102] In some embodiments, the robot generates a reference trajectory x during the path planning process. r (t), when the robot moves along the reference trajectory x r (t) When in motion, the following relationship should be satisfied:

[0103] x t (t) = f(θ r (t)); (6)

[0104] Among them, θ r (t) is the reference trajectory x r (t) Corresponding joint motion trajectory.

[0105] During the actual operation of the robot arm, its joint angles are subject to certain restrictions. That is, the generated joint angles should not exceed their physical limits, and corresponding constraints should be set:

[0106]

[0107] Among them, g(x r (t),θ(t)) represents the objective function, m represents the number of robot joints, Indicates the minimum and maximum limit angles of the kth joint of the robot. At this time, a tracking error e is defined, and its value is e = x r (t)-f(θ(t)), and put it into formula (7), so that formula (7) is transformed into:

[0108]

[0109] stθ - <θ<θ + ; (8)

[0110] After the joint of the robot arm moves to the next position, the direction vector, antenna coordinates and objective function value are updated, and the process is repeated until the path planning is completed.

[0111] As mentioned above, the robot can obtain the path trajectory through the position and direction data of the end effector. Therefore, the tracking error e is defined as:

[0112] In formula (9), is the real Cartesian coordinate of the end effector. Substituting formula (9) into formula (7), we can get the objective function:

[0113]

[0114] During the movement of the robot arm, an instantaneous time value t is defined k The joint coordinates at this time are θ k , simulates the beetle antenna to generate a normally distributed random direction vector According to the direction vector The coordinates of the endpoints of the two antennas are calculated as:

[0115]

[0116] In the above formula (11), θ L and θ R represents the beetle at time t k The projection positions of the left and right antennas below. k is a hyperparameter, which indicates the length of the beetle antenna. Γ(.) is a projection function, which means that these points are projected into the constraint space defined by the objective function and applied to each joint angle in turn. Its value is:

[0117]

[0118] The robot moves the joints to θ in sequence L and θ R The coordinates are located at the position of the target point, and the value of the objective function is calculated using the following formula:

[0119]

[0120] At this time, x r (t) is t k The coordinates of the points on the reference trajectory at time , t k The position coordinates of the end effector at the time instant, so as to calculate the objective function value g of the left and right antennas of the beetle L and g R Then, the objective function value is used to obtain the robot's forward direction and update the joint position θ for the next step. new , and its update strategy is:

[0121]

[0122] in, Indicates that the joint update position of the robot arm moves towards the direction of the antenna with a smaller objective function value, δ k (λ k ) represents the updated θ new With the current joint position θk The Euclidean distance between them, k is the sample index, which affects the convergence speed of the entire algorithm. k and δ k (λ k ) can be:

[0123]

[0124] δ k (λ k )=c 2 λ k ; (16)

[0125] At this time, the convergence speed of the algorithm is moderate and the effect is good. In formula (15) and formula (16), c 1 and c 2 is a constant used to dynamically adjust the step size between the end effector and the target position. When the end effector is far away from the target position, the algorithm step size is larger, and when the end effector is close to the target position, the step size is smaller. 1 and c 2 The present invention proposes the following fast convergence strategy:

[0126] c 1 ∝T s ,c 2 ∈[1,3]; (17)

[0127] Among them, T s is the sampling time of the control loop. When the robot reaches θ new After that, update the value of the objective function:

[0128]

[0129] G new Compared with the objective function value at the initial position. new The value of is small, then the robot moves to θ new position, otherwise, return θ k .

[0130]

[0131] When the robot arm's θ k Move to θ k+1 After that, the direction vector, antenna coordinates, and objective function value are updated again, and the above process is repeated.

[0132] The meta-heuristic robot path fast search and obstacle avoidance method of the present invention adopts Gaussian mixture distribution and dynamic target bias probability strategy to sample random points and guide the random tree to expand to the target point.

[0133] The sampling point closest to the target point is selected through multiple sampling, which reduces the algorithm's exploration of unnecessary space and avoids the expansion and generation of invalid directions.

[0134] The variable step size expansion method allows the search tree to quickly find paths with fewer nodes and iterations.

[0135] An angle restriction constraint strategy is introduced. When the angle between the line connecting the previous sampling point and the current point is equal to the angle between the line connecting the starting point and the end point, the current point is used as a new sampling point and added to the random tree.

[0136] In summary, the present invention can improve the generation speed and sampling efficiency of the planned path, and the obstacle avoidance search process requires that the joint space is fully controllable without breaking the physical limits of the joint control position and speed, thereby ensuring the safe operation of the robotic arm.

Claims

1. A meta-heuristic method for rapid path search and obstacle avoidance of a robotic arm, characterized in that: include: (1) During the path planning process, the robot arm uses an improved Gaussian mixture distribution sampling strategy to impose interval constraints on the generation of random trees to improve the generation speed of the planned path. (2) The robot arm introduces a map complexity assessment strategy during the sampling process to reduce the time spent on finding the optimal path point; (3) A hybrid sampling strategy is implemented by integrating the improved Gaussian mixture distribution sampling strategy and the map complexity constraint strategy.

2. The meta-heuristic robot path fast search and obstacle avoidance method according to claim 1 is characterized in that: In the improved Gaussian mixture distribution sampling strategy, the n-dimensional function expression of the Gaussian function is as follows: σ is the standard deviation of the Gaussian distribution function, representing the distance between the sampling point and the obstacle, and μ is the expected value; The probability distribution function of the sampling point in the obstacle area is: φ(x;μ,σ)=∫f(x;μ,σ)dx; (2) From the above probability distribution function, we can see that the closer to the obstacle, the greater the sampling probability. In order to avoid collision between the sampling point and the obstacle, the obstacle function obs(x) is set and the probability distribution function is improved. The improved probability distribution function expression is as follows: g(x;μ,σ)=max(0,φ(x;μ,σ)-obs(x)); (3) Wherein, obs(x) is the obstacle function. When the generated sampling point collides with an obstacle, obs(x)=1, otherwise it is 0. If the sampling point falls in the obstacle-free area, g(x; μ, σ)=φ(x; μ, σ), otherwise g(x; μ, σ)=0.

3. The meta-heuristic robot path fast search and obstacle avoidance method according to claim 2, characterized in that: The robot arm introduces a map complexity evaluation strategy during the sampling process to reduce the time of finding the optimal path point; at the same time, the constraint threshold p is set; Among them, the map complexity coefficient C1 is expressed as: 0.5 represents the weight ratio, S obc Represents the obstacle area, S map Indicates the map area, D obc Indicates the distribution of obstacles.

4. The meta-heuristic robot path fast search and obstacle avoidance method according to claim 3 is characterized in that: The specific implementation steps of the hybrid sampling strategy that integrates the improved Gaussian mixture distribution sampling strategy and the map complexity constraint strategy are as follows: (1) Set the constraint threshold p. If the probability of generating a sampling point is less than the constraint threshold p, a point Q is randomly generated. rand1 Added random trees; (2) If Q rand1 If a collision occurs with an obstacle, a point Q is regenerated through the improved probability distribution function rand2 ; If no collision occurs, return Q rand1 ; (3) If Q rand2 If it collides with an obstacle, select Q rand1 and Q rand2 The midpoint Q in space rand3 If no collision occurs, return Q rand2 ; (4) If Q rand3 If there is no collision with the obstacle, then Q rand3 is the sampling point of the random tree, otherwise, return to the first step and resample.

5. The meta-heuristic robot path fast search and obstacle avoidance method according to claim 4, characterized in that: The robot arm uses a hybrid sampling strategy to introduce a sampling constraint strategy during path planning. The sampling constraint strategy specifically includes: The angle θ between the line connecting the previous sampling point and the current point and the line connecting the starting point and the end point is used as the criterion for judging the positive growth of the sampling point; when the angle θ∈[-π / 2,π / 2], it is judged that the random sampling point belongs to positive growth; when the angle θ is not in the range of [-π / 2,π / 2], the random sampling point does not meet the sampling constraint requirements (i.e., it belongs to reverse growth), and the random sampling point should be eliminated.

6. The meta-heuristic robot path fast search and obstacle avoidance method according to claim 5, characterized in that: The robot adopts a hybrid sampling strategy and a sampling constraint strategy during the path planning process, and adopts an adaptive step size strategy to generate a new node Q new , to reduce the probability of collision between the newly extended node and obstacles.

7. The meta-heuristic robot path fast search and obstacle avoidance method according to claim 6, characterized in that: The new node Q new Determined by the following formula: Where ρ1 is Q new The step size coefficient towards the random sampling point, ρ2 is Q new Step coefficient towards the target point. When ρ1>ρ2, the new node Q new Grow towards the position and direction of the random sampling point; when ρ1<ρ2, the new node Q new Grow towards the target point position and direction.

8. The meta-heuristic robot path fast search and obstacle avoidance method according to claim 7, characterized in that: The robot generates a reference trajectory x during the path planning process. r (t), when the robot moves along the reference trajectory x r (t) When in motion, the following relationship should be satisfied: x t (t)=f(θ r (t)); (6) Among them, θ r (t) is the reference trajectory x r (t) Corresponding joint motion trajectory.

9. The meta-heuristic robot path fast search and obstacle avoidance method according to claim 8, characterized in that: The robot moves along the reference trajectory x r (t) During motion, the generated joint angles should not exceed their physical limits, and corresponding constraints should be set: Among them, g(x r (t),θ(t)) represents the objective function, m represents the number of robot joints, represents the minimum and maximum limit angles of the kth joint of the robot arm; at this time, a tracking error e is defined, and its value is e = x r (t)-f(θ(t)), and put it into formula (7), so that formula (7) is transformed into: stθ - <θ<θ + 。 (8) 10. The meta-heuristic robot path fast search and obstacle avoidance method according to claim 8, characterized in that: After the joint of the robot arm moves to the next position, the direction vector, antenna coordinates and objective function value are updated, and the process is repeated until the path planning is completed.

Images