Robot obstacle crossing path planning method based on improved bidirectional RRT

Abstract

The invention discloses a robot obstacle crossing path planning method based on improved bidirectional RRT. The method comprises the following steps: step 1, setting a target point Pgoal; 2, two candidate sampling points Pand1 and Pand2 are randomly generated in a bidirectional mode; 3, comparing the distances from the two candidate sampling points Pand1 and Pand2 to the target point Pgoal, if Pand1, Pgoallt, and if Pand1, Pgoallt; selecting the Prand1 as a final sampling point Prand1, Prand2 and Pgoal, otherwise, selecting the Prand2 as a final sampling point Prand1; 4, calculating a motion path of the robot from the current node to a new sampling point; 5, checking whether a collision obstacle exists on the motion path or not, and adopting a corresponding obstacle avoidance strategy; and step 6, carrying out smoothing processing on the planned numerous segments of paths. According to the improved bidirectional RRT algorithm provided by the invention, the target guidance quality of the improved bidirectional RRT algorithm is enhanced due to a double-sampling-point method, so that the average planning path length of the improved bidirectional RRT algorithm is shortened to a certain extent, the movement of the robot is more effective, whether collision obstacles exist on the movement path or not can be checked, and a corresponding obstacle avoidance strategy is adopted.

Description

Technical Field

[0001] The present invention relates to the technical field of robot obstacle-crossing path planning, and in particular to a robot obstacle-crossing path planning method based on improved bidirectional RRT. Background Art

[0002] The traditional inspection and maintenance methods of angle steel towers require personnel to work at heights, which poses a safety hazard. At the same time, the work efficiency is relatively low and it is difficult to meet high-precision requirements. Nowadays, angle steel tower climbing robots are mainly used for work. Angle steel tower climbing robots are robots used for climbing, inspection and maintenance on high-altitude angle steel structures.

[0003] However, since obstacles of various shapes and sizes may appear on the surface of the angle steel tower, such as rust, gravel, weeds, rivets, screws, etc., these obstacles will hinder the movement of the robot, making it unable to successfully complete the climbing task, and even cause danger or damage. Therefore, the present application provides a robot obstacle path planning method based on improved bidirectional RRT. Obstacle path planning can help the angle steel tower climbing robot automatically avoid obstacles during the climbing process and select an optimal path, thereby improving the robot's work efficiency and safety. Summary of the invention

[0004] The purpose of this section is to summarize some aspects of embodiments of the present invention and briefly introduce some preferred embodiments. Some simplifications or omissions may be made in this section and the specification abstract and the invention title of this application to avoid blurring the purpose of this section, the specification abstract and the invention title, and such simplifications or omissions cannot be used to limit the scope of the present invention.

[0005] In view of the above-mentioned problem of a robot obstacle-crossing path planning method based on improved bidirectional RRT, the present invention is proposed.

[0006] Therefore, the purpose of the present invention is to provide a robot obstacle path planning method based on improved bidirectional RRT, which is used to solve the problem that obstacles of various shapes and sizes may appear on the surface of the angle steel tower, such as rust, gravel, weeds, rivets, screws, etc. These obstacles will cause the movement of the robot to be hindered, making it unable to successfully complete the climbing task, and even cause danger or damage.

[0007] In order to solve the above technical problems, the present invention provides the following technical solutions: a robot obstacle path planning method based on improved bidirectional RRT, comprising the following steps:

[0008] Step 1: Set the target point Pgoal;

[0009] Step 2: Generate two candidate sampling points Prand1 and Prand2 randomly in both directions;

[0010] Step 3: Compare the distances between the two candidate sampling points Prand1 and Prand2 and the target point Pgoal. If |Prand1,Pgoal|<|Prand2,Pgoal|, then select Prand1 as the final sampling point Prand, otherwise select Prand2.

[0011] Step 4: Calculate the robot's motion path from the current node to the new sampling point;

[0012] Step 5: Check whether there are any collision obstacles on the motion path and adopt corresponding obstacle avoidance strategies;

[0013] Step 6: Smooth the planned multiple path segments.

[0014] As a preferred solution of the robot obstacle path planning method based on improved bidirectional RRT described in the present invention, in step 4, the robot's motion path includes linear motion and turning motion.

[0015] As a preferred solution of the robot obstacle path planning method based on improved bidirectional RRT described in the present invention, the linear motion uses the following kinematic model to calculate the trajectory of the robot from the current node to the new sampling point:

[0016] x(t)=x0+v*t

[0017] y(t)=y0+v*t

[0018] θ(t)=θ0

[0019] Among them, (x0, y0) is the current node position, v is the linear velocity of the robot, t is the time, and θ0 is the current posture angle of the robot;

[0020] The turning motion uses the following kinematic model to calculate the trajectory of the robot from the current node to the new sampling point:

[0021] x(t)=x c +R*cos(θ+wt)

[0022] y(t)=y c +R*sin(θ+wt)

[0023] When the robot is turning, its path is usually an arc. When the robot is turning, (x c ,y c ) is the center of the circle, the direction angle is θ, and the radius is R for turning motion, where t is time and w is angular velocity.

[0024] As a preferred solution of the robot obstacle path planning method based on improved bidirectional RRT described in the present invention, in step five, a ray casting collision detection method is used to emit rays from a point to an object, and then check whether the ray intersects with the surface of the object to perform collision detection. If the ray intersects with the surface of the object, it means that there is a collision.

[0025] As a preferred solution of the robot obstacle-crossing path planning method based on improved bidirectional RRT described in the present invention, a corresponding obstacle avoidance strategy is adopted when a collision is detected, and the obstacle avoidance strategy includes path planning obstacle avoidance and action obstacle avoidance.

[0026] As a preferred solution of the robot obstacle path planning method based on improved bidirectional RRT described in the present invention, the path planning obstacle avoidance includes stopping and replanning the path and initial path obstacle avoidance planning;

[0027] Stop and re-plan the path means that when a collision is detected, the robot can immediately stop moving and re-plan the path to bypass the obstacle;

[0028] Initial path obstacle avoidance planning means taking obstacles into consideration during the path planning stage and generating a path that avoids obstacles.

[0029] As a preferred solution of the robot obstacle path planning method based on improved bidirectional RRT described in the present invention, the action obstacle avoidance includes dynamic obstacle avoidance and local obstacle avoidance;

[0030] Dynamic obstacle avoidance means that the robot adjusts its speed, direction or posture in real time to bypass obstacles;

[0031] Local obstacle avoidance means that obstacles only exist in specific areas. LiDAR obstacle avoidance is used to move along the edge of obstacles or through narrow passages.

[0032] As a preferred solution of the robot obstacle path planning method based on improved bidirectional RRT described in the present invention, the laser radar obstacle avoidance method refers to that the robot is equipped with a laser radar sensor, and by analyzing the laser radar data, the obstacles in front of the robot can be identified, and the direction or speed of the robot can be adjusted to avoid the obstacles.

[0033] As a preferred solution of the robot obstacle-crossing path planning method based on improved bidirectional RRT described in the present invention, the spline curve function formed after smoothing the path in step six is:

[0034]

[0035] Among them, pu(t) is the final smooth curve, P iis the control vertex (the node generated by the smooth path), i is the i-th node generated by the smooth path, k is the spline order, t represents the node weight (0-1), N i,k(t) is the k-order canonical spline basis function.

[0036] Beneficial effects of the present invention: The improved bidirectional RRT algorithm proposed in the present invention enhances its goal orientation due to the dual sampling point method, so that its average planned path length is also shortened to a certain extent, making the robot movement more efficient. In addition, the present invention can check whether there are collision obstacles on the movement path and adopt corresponding obstacle avoidance strategies, which further proves the superiority of the improved bidirectional RRT algorithm of the present invention. BRIEF DESCRIPTION OF THE DRAWINGS

[0037] In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the accompanying drawings required for use in the description of the embodiments will be briefly introduced below. Obviously, the accompanying drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other accompanying drawings can be obtained based on these accompanying drawings without paying creative labor.

[0038] in:

[0039] Figure 1 This is a pseudo-code algorithm diagram of a sampling algorithm of a robot obstacle-crossing path planning method based on improved bidirectional RRT in the present invention.

[0040] Figure 2 The present invention is a schematic diagram of path smoothing processing of a robot obstacle-crossing path planning method based on improved bidirectional RRT.

[0041] Figure 3 This is a comparison table of simulation test data of a robot obstacle-crossing path planning method based on improved bidirectional RRT of the present invention. DETAILED DESCRIPTION

[0042] In order to make the above-mentioned objects, features and advantages of the present invention more obvious and easy to understand, the specific implementation methods of the present invention are described in detail below in conjunction with the accompanying drawings.

[0043] In the following description, many specific details are set forth to facilitate a full understanding of the present invention, but the present invention may also be implemented in other ways different from those described herein, and those skilled in the art may make similar generalizations without violating the connotation of the present invention. Therefore, the present invention is not limited to the specific embodiments disclosed below.

[0044] Secondly, the term "one embodiment" or "embodiment" as used herein refers to a specific feature, structure, or characteristic that may be included in at least one implementation of the present invention. The term "in one embodiment" that appears in different places in this specification does not necessarily refer to the same embodiment, nor does it refer to a separate or selective embodiment that is mutually exclusive with other embodiments.

[0045] Secondly, the present invention is described in detail with reference to the schematic diagram. When describing the embodiments of the present invention in detail, for the sake of convenience, the cross-sectional diagrams showing the device structure will not be partially enlarged according to the general scale, and the schematic diagrams are only examples, which should not limit the scope of protection of the present invention. In addition, in actual production, the three-dimensional dimensions of length, width and depth should be included.

[0046] Reference Figure 1-Figure 2 and Figure 3 , as an embodiment of the present invention, provides a robot obstacle path planning method based on improved bidirectional RRT, which includes the following steps:

[0047] Step 1: Set the target point Pgoal;

[0048] Step 2: Generate two candidate sampling points Prand1 and Prand2 randomly in both directions;

[0049] Step 3: Compare the distances between the two candidate sampling points Prand1 and Prand2 and the target point Pgoal. If |Prand1,Pgoal|<|Prand2,Pgoal|, then select Prand1 as the final sampling point Prand, otherwise select Prand2.

[0050] Step 4: Calculate the robot's motion path from the current node to the new sampling point;

[0051] Step 5: Check whether there are any collision obstacles on the motion path and adopt corresponding obstacle avoidance strategies;

[0052] Step 6: Smooth the planned multiple path segments.

[0053] Among them, the sampling algorithm pseudo code algorithm of step three is as follows Figure 1 As shown, in step 4, the robot's motion path includes linear motion and turning motion. The linear motion uses the following kinematic model to calculate the trajectory of the robot from the current node to the new sampling point:

[0054] x(t)=x0+v*t

[0055] y(t)=y0+v*t

[0056] θ(t)=θ0

[0057] Among them, (x0, y0) is the current node position, v is the linear velocity of the robot, t is the time, and θ0 is the current posture angle of the robot;

[0058] The turning motion uses the following kinematic model to calculate the trajectory of the robot from the current node to the new sampling point:

[0059] x(t)=x c +R*cos(θ+wt)

[0060] y(t)=y c +R*sin(θ+wt)

[0061] When the robot is turning, its path is usually an arc. When the robot is turning, (x c ,y c ) is the center of the circle, the direction angle is θ, and the radius is R for turning motion, where t is time and w is angular velocity.

[0062] In step five, a ray casting collision detection method is used to emit a ray from a point to an object, and then a collision detection is performed by checking whether the ray intersects with the surface of the object. If the ray intersects with the surface of the object, it means that a collision exists.

[0063] When a collision is detected, the corresponding obstacle avoidance strategy is adopted. The obstacle avoidance strategy includes path planning obstacle avoidance and action obstacle avoidance. Path planning obstacle avoidance includes stopping and replanning the path and initial path obstacle avoidance planning. Among them, stopping and replanning the path means that when a collision is detected, the robot can stop moving immediately and replan the path to bypass the obstacle; initial path obstacle avoidance planning means that obstacles are considered in the path planning stage and a path to avoid obstacles is generated. Action obstacle avoidance includes dynamic obstacle avoidance and local obstacle avoidance. Dynamic obstacle avoidance means that the robot adjusts its speed, direction or posture in real time to bypass obstacles. Local obstacle avoidance means that obstacles only exist in specific areas, and the laser radar obstacle avoidance method is used to move along the edge of the obstacle or through narrow passages. The laser radar obstacle avoidance method means that the robot is equipped with a laser radar sensor. By analyzing the laser radar data, it can identify obstacles in front of the robot and adjust the robot's direction or speed to avoid obstacles.

[0064] The spline curve function formed after smoothing the path in step 6 is:

[0065]

[0066] Among them, pu(t) is the final smooth curve, P i is the control vertex (the node generated by the smooth path), i is the i-th node generated by the smooth path, k is the spline order, t represents the node weight (0-1), N i,k(t)It is the k-order canonical spline basis function, which is obtained by the recursive formula of the canonical basis function. The 0-order canonical spline basis function is known, and the entire basis function basic function table is known, so the formula means that the final curve position is the product of the weight values ​​of all elements under the k-th order in the basic function table and the coordinates of the corresponding position control point.

[0067] This embodiment selects a quadratic spline curve, such as Figure 2 As shown in the figure, three discrete sampling points P0, P1, and P2 are known in the plane. The quadratic parabola segment is defined by these three points. The matrix form of the parametric equation of the quadratic spline curve is:

[0068]

[0069] When there are more than 3 discrete points, quadratic spline curves are used to perform piecewise fitting in sequence. At the same time, boundary problems can be considered. A quadratic spline curve starting from the starting point and ending at the end point can be drawn through the original discrete points.

[0070] For the improved two-way RRT algorithm, it is necessary to first verify the impact of the improvement method on its performance and effectiveness through software simulation. The most intuitive standard is the algorithm running time and the final planned path length under the same map environment. This project first conducted simulation tests on a general two-dimensional plane. On a PC with an Intel(R) Core(TM) i5-8400 CPU@3.30GHz 16G memory and a 64-bit Windows 10 operating system, Matlab2018 was used to run the standard RRT algorithm, RRT-Connect algorithm, and the improved two-way RRT algorithm of this project. A statistical comparative analysis of these algorithms was performed on indicators such as path planning time and final path length in two-dimensional map planning.

[0071] Depend on Figure 3 As shown, the statistical data of the simulation test show that: compared with the standard RRT algorithm, the path planning time of the RRT Connect algorithm is shortened by more than 46%, and the path planning time of the improved two-way RRT algorithm of the present invention is shortened by more than 42% compared with the RRT Connect algorithm. In terms of the average planned path length, there is no obvious difference between the standard RRT algorithm and the RRT Connect algorithm. However, the improved two-way RRT algorithm proposed in the present invention enhances its goal orientation due to the double sampling point method, so that its average planned path length is also shortened to a certain extent, making the robot movement more efficient, which further proves the superiority of the improved two-way RRT algorithm of the present invention.

[0072] 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 preferred embodiments, those skilled in the art should understand that the technical solutions of the present invention may be modified or replaced by equivalents without departing from the spirit and scope of the technical solutions of the present invention, which should all be included in the scope of the claims of the present invention.

Claims

1. A robot obstacle path planning method based on improved bidirectional RRT, characterized in that: The following steps are involved: Step 1: Set the target point Pgoal; Step 2: Generate two candidate sampling points Prand1 and Prand2 randomly in both directions; Step 3: Compare the distances between the two candidate sampling points Prand1 and Prand2 and the target point Pgoal. If |Prand1,Pgoal|<|Prand2,Pgoal|, then select Prand1 as the final sampling point Prand, otherwise select Prand2. Step 4: Calculate the robot's motion path from the current node to the new sampling point; Step 5: Check whether there are any collision obstacles on the motion path and adopt corresponding obstacle avoidance strategies; Step 6: Smooth the planned multiple path segments.

2. The robot obstacle path planning method based on improved bidirectional RRT according to claim 1, characterized in that: In step 4, the robot's motion path includes linear motion and turning motion.

3. The robot obstacle path planning method based on improved bidirectional RRT according to claim 2, characterized in that: Linear motion uses the following kinematic model to calculate the trajectory of the robot from the current node to the new sampling point: x(t)=x0+v*t y(t)=y0+v*t θ(t)=θ0 Among them, (x0, y0) is the current node position, v is the linear velocity of the robot, t is the time, and θ0 is the current posture angle of the robot; The turning motion uses the following kinematic model to calculate the trajectory of the robot from the current node to the new sampling point: x(t)=x c +R*cos(θ+wt) y(t)=y c +R*sin(θ+wt) When the robot is turning, its path is usually an arc. When the robot is turning, (x c ,y c ) is the center of the circle, the direction angle is θ, and the radius is R for turning motion, where t is time and w is angular velocity.

4. The robot obstacle path planning method based on improved bidirectional RRT according to claim 1, characterized in that: In step five, a ray casting collision detection method is used to emit a ray from a point to an object, and then a collision detection is performed by checking whether the ray intersects with the surface of the object. If the ray intersects with the surface of the object, it means that a collision exists.

5. The robot obstacle-crossing path planning method based on improved bidirectional RRT according to claim 4, characterized in that: When a collision is detected, a corresponding obstacle avoidance strategy is adopted, which includes path planning obstacle avoidance and action obstacle avoidance.

6. The robot obstacle-crossing path planning method based on improved bidirectional RRT according to claim 5, characterized in that: Path planning and obstacle avoidance include stopping and replanning the path and initial path obstacle avoidance planning; Stop and re-plan the path means that when a collision is detected, the robot can immediately stop moving and re-plan the path to bypass the obstacle; Initial path obstacle avoidance planning means taking obstacles into consideration during the path planning stage and generating a path that avoids obstacles.

7. The robot obstacle-crossing path planning method based on improved bidirectional RRT according to claim 6, characterized in that: Action obstacle avoidance includes dynamic obstacle avoidance and local obstacle avoidance; Dynamic obstacle avoidance means that the robot adjusts its speed, direction or posture in real time to bypass obstacles; Local obstacle avoidance means that obstacles only exist in specific areas. LiDAR obstacle avoidance is used to move along the edge of obstacles or through narrow passages.

8. The robot obstacle-crossing path planning method based on improved bidirectional RRT according to claim 7, characterized in that: LiDAR obstacle avoidance means that the robot is equipped with a LiDAR sensor. By analyzing the LiDAR data, it can identify obstacles in front of the robot and adjust the direction or speed of the robot to avoid the obstacles.

9. The robot obstacle path planning method based on improved bidirectional RRT according to claim 1, characterized in that: The spline curve function formed after smoothing the path in step 6 is: Among them, pu(t) is the final smooth curve, P i is the control vertex (the node generated by the smooth path), i is the i-th node generated by the smooth path, k is the spline order, t represents the node weight (0-1), N i,k(t) is the k-order canonical spline basis function.

Images