阅读说明：本技术 弓形路径无缝衔接规划方法 (Seamless connection planning method for arcuate path ) 是由 杜元源 于 2020-10-29 设计创作，主要内容包括：本发明公开了一种弓形路径无缝衔接规划方法,包括如下步骤：读取智能车当前所在的区域为待规划区域,以智能车当前位置为起始点；找出待规划区域横向线段的右上角点、左上角点、右下角点、左下角点,计算起始点与角点的距离；取距离最近的点作为路径起点,然后连接该点所在线段的另一端到该区域相邻线段的距离较近一端,依次连接该区域所有线段；剩余区域的每个区域的角点,将第上一步连接所得曲线的最后一个点作为区域终点,分别计算区域终点到剩余每个区域角点的距离,距离最近的点作为下一个待规划区域的起始点,其所在区域为下一个待规划区域；连接区域终点到待规划区域的起始点；通过所述方法规划的路径更加笔直有效,覆盖率高,同时智能车在追踪该方法规划的路径更为容易。(The invention discloses a method for planning seamless connection of an arch path, which comprises the following steps: reading an area where the intelligent vehicle is located currently as an area to be planned, and taking the current position of the intelligent vehicle as a starting point; finding out an upper right corner, an upper left corner, a lower right corner and a lower left corner of a transverse line segment of a region to be planned, and calculating the distance between an initial point and the corners; taking the closest point as a path starting point, then connecting the other end of the line segment where the point is located to the closer end of the adjacent line segment in the area, and sequentially connecting all the line segments in the area; taking the last point of the curve obtained by the last step of connection as an area terminal point, respectively calculating the distance from the area terminal point to each remaining area corner point, taking the point with the closest distance as the starting point of the next area to be planned, and taking the area where the point is located as the next area to be planned; connecting the area terminal point to the starting point of the area to be planned; the path planned by the method is straighter and more effective, the coverage rate is high, and meanwhile, the intelligent vehicle can track the path planned by the method more easily.)

1. An arch path seamless connection planning method is characterized by comprising the following steps:

1) constructing a cost map of the movement of the intelligent vehicle, and expanding the barriers and the movement boundary in the cost map by r pixel values, wherein r is the radius of the intelligent vehicle;

2) dividing the whole cost map into a plurality of small areas according to the obstacles and the movement boundaries in the map;

3) for each small-area horizontal line, the rule is that from top to bottom, the distance between the first line and the upper boundary is r, the distance between the lines is 2r, and until the distance between the line and the lower boundary is less than r, a line is drawn at the position which is above the lower boundary and has the distance of r;

4) taking the current area of the intelligent vehicle as an area to be planned, and taking the current position of the intelligent vehicle as a starting point;

5) finding out an upper right corner, an upper left corner, a lower right corner and a lower left corner of a transverse line segment of a region to be planned, and calculating the distance between an initial point and the corners; taking the closest point as a path starting point, then connecting the other end of the line segment where the point is located to the closer end of the adjacent line segment in the area, and sequentially connecting all the line segments in the area;

6) taking the corner points of each region of the remaining region, taking the last point of the curve obtained by the last step of connection as a region terminal point, respectively calculating the distance from the region terminal point to the corner points of each remaining region, taking the point with the closest distance as the starting point of the next region to be planned, and taking the region where the point is located as the next region to be planned; connecting the area terminal point to the starting point of the area to be planned;

7) and repeating the steps 4) to 6) until all the areas are connected.

2. The arcuate path seamless join planning method of claim 1, wherein: the method for dividing the whole cost map into a plurality of small areas is as follows:

1) finding out boundary lines of the regions to be divided by using Opencv software, calculating the length of each line segment according to formula (1) and calculating a direction angle according to formula (2);

length of wire section

Direction angleWherein x₂Is not equal to x₁If x is₂＝x₁Then theta is pi/2 (2)

2) Classifying according to direction angles, wherein the angles are from zero degrees to 180 degrees, every 30 degrees are classified into one class, and the classes are totally classified into 6 classes; respectively calculating the sum of the length of each line segment, and calculating the final rotation direction angle theta of the longest line segment according to the formula (3)₀；

Angle of rotation theta₀＝(θ₁*L₁+θ₂*L₂+θ₃*L₃+...+θ_n*L_n)/n (3)

Wherein n is the number of line segments;

3) rotating map by-theta using opencv₀When the longer side of the boundary of the map is parallel to the x axis;

4) reading pixel values of the cost map from top to bottom from left to right, and recording the number of continuous white line segments of each line; counting from the line with at least 1 white line segment until the whole picture is calculated;

5) judging the change condition of the number of the continuous line segments, if the number of the line segments in the upper line is K more than that of the line segments in the line, K downward convex critical points exist in the upper line; if the number of the line segments in the previous line is less than that of the line segments in the previous line by K, K upwards-convex critical points exist in the line; recording the line number of the critical point and recording the number of the critical point;

6) sequentially taking the rows with the critical points, sequentially taking the critical points of the rows, and filling black to the left pixel by pixel from the critical points until the pixel points to be filled are black; sequentially filling black pixels to the right until the pixel points to be filled are black; processing of the single critical point is finished, and then each critical point is processed according to the mode;

7) and obtaining a final effect picture, wherein each white connected region is a partition.

Technical Field

The invention relates to the technical field of robot navigation obstacle avoidance, in particular to a seamless connection planning method for an arch path.

Background

With the vigorous development of intelligent robot technology, robots are widely applied in production and life. According to different application environments, robots can be classified into various categories, for example, industrial robots often have a plurality of joints and mechanical arms, and are subjected to efficient work such as assembly, transportation and the like in factories; service robot products including wheel type mobile robots, humanoid robots and the like can be used in household or commercial environments, different functions are developed aiming at different use purposes, the intelligent mobile car can realize unmanned driving, and the humanoid robots can realize singing, dancing, entertainment and interaction and the like; in the robot real-time obstacle avoidance algorithm, a dynamic cost map is of great importance, the dynamic cost map shows obstacles detected by a sensor in real time, the obstacle information around the mobile robot can be detected by combining an established static map, and then a shortest path capable of reaching a destination is calculated by combining a path planning algorithm.

Disclosure of Invention

The invention aims to solve the technical problem of how to provide a seamless path planning method which has straighter and more effective path and high coverage rate and is easier for an intelligent vehicle to track the path planned by the method.

In order to solve the technical problems, the technical scheme adopted by the invention is as follows: an arch path seamless connection planning method is characterized by comprising the following steps:

1) constructing a cost map of the movement of the intelligent vehicle, and expanding the barriers and the movement boundary in the cost map by r pixel values, wherein r is the radius of the intelligent vehicle;

2) dividing the whole cost map into a plurality of small areas according to the obstacles and the movement boundaries in the map;

3) for each small-area horizontal line, the rule is that from top to bottom, the distance between the first line and the upper boundary is r, the distance between the lines is 2r, and until the distance between the line and the lower boundary is less than r, a line is drawn at the position which is above the lower boundary and has the distance of r;

4) taking the current area of the intelligent vehicle as an area to be planned, and taking the current position of the intelligent vehicle as a starting point;

5) finding out an upper right corner, an upper left corner, a lower right corner and a lower left corner of a transverse line segment of a region to be planned, and calculating the distance between an initial point and the corners; taking the closest point as a path starting point, then connecting the other end of the line segment where the point is located to the closer end of the adjacent line segment in the area, and sequentially connecting all the line segments in the area;

6) taking the corner points of each region of the remaining region, taking the last point of the curve obtained by the last step of connection as a region terminal point, respectively calculating the distance from the region terminal point to the corner points of each remaining region, taking the point with the closest distance as the starting point of the next region to be planned, and taking the region where the point is located as the next region to be planned; connecting the area terminal point to the starting point of the area to be planned;

7) and repeating the steps 4) to 6) until all the areas are connected.

Adopt the produced beneficial effect of above-mentioned technical scheme to lie in: the method realizes seamless coverage path planning according to the cost map, compared with other planning methods, the method has the advantages that the planned path is more straight and effective, the coverage rate is high, and meanwhile, the intelligent vehicle can track the path planned by the method more easily.

Drawings

The present invention will be described in further detail with reference to the accompanying drawings and specific embodiments.

FIG. 1 is a cost map in a method according to an embodiment of the present invention;

fig. 2 is a diagram of a path planned by the method.

Detailed Description

The technical solutions in the embodiments of the present invention are clearly and completely described below with reference to the drawings in the embodiments of the present invention, and it is obvious that the described embodiments are only a part of the embodiments of the present invention, and not all of the embodiments. All other embodiments, which can be derived by a person skilled in the art from the embodiments given herein without making any creative effort, shall fall within the protection scope of the present invention.

In the following description, numerous specific details are set forth in order to provide a thorough understanding of the present invention, but the present invention may be practiced in other ways than those specifically described and will be readily apparent to those of ordinary skill in the art without departing from the spirit of the present invention, and therefore the present invention is not limited to the specific embodiments disclosed below.

The embodiment of the invention discloses a method for planning seamless connection of an arch path, which comprises the following steps:

1) constructing a cost map of the movement of the intelligent vehicle, and expanding the barriers and the movement boundary in the cost map by r pixel values, wherein r is the radius of the intelligent vehicle;

2) dividing the whole cost map into a plurality of small areas according to the obstacles and the movement boundaries in the map;

3) for each small-area horizontal line, the rule is that from top to bottom, the distance between the first line and the upper boundary is r, the distance between the lines is 2r, and until the distance between the line and the lower boundary is less than r, a line is drawn at the position which is above the lower boundary and has the distance of r;

4) taking the current area of the intelligent vehicle as an area to be planned, and taking the current position of the intelligent vehicle as a starting point;

5) finding out an upper right corner, an upper left corner, a lower right corner and a lower left corner of a transverse line segment of a region to be planned, and calculating the distance between an initial point and the corners; taking the closest point as a path starting point, then connecting the other end of the line segment where the point is located to the closer end of the adjacent line segment in the area, and sequentially connecting all the line segments in the area;

6) taking the corner points of each region of the remaining region, taking the last point of the curve obtained by the last step of connection as a region terminal point, respectively calculating the distance from the region terminal point to the corner points of each remaining region, taking the point with the closest distance as the starting point of the next region to be planned, and taking the region where the point is located as the next region to be planned; connecting the area terminal point to the starting point of the area to be planned;

7) repeating the steps 4) to 6) until all the areas are connected;

8) so far, a path planning of seamless coverage with the current position of the vehicle as the starting point is completed, as shown in fig. 2.

Further, the method for dividing the whole cost map into a plurality of small areas is as follows:

1) finding out boundary lines of the regions to be divided by using Opencv software, calculating the length of each line segment according to formula (1) and calculating a direction angle according to formula (2);

length of wire section

Direction angleWherein x₂Is not equal to x₁If x is₂＝x₁Then theta is pi/2 (2)

2) Classifying according to direction angles, wherein the angles are from zero degrees to 180 degrees, every 30 degrees are classified into one class, and the classes are totally classified into 6 classes; respectively calculating the sum of the length of each line segment, and calculating the final rotation direction angle theta of the longest line segment according to the formula (3)₀；

Angle of rotation theta₀＝(θ₁*L₁+θ₂*L₂+θ₃*L₃+...+θ_n*L_n)/n (3)

Wherein n is the number of line segments;

3) rotating map by-theta using opencv₀When the longer side of the boundary of the map is parallel to the x axis;

4) reading pixel values of the cost map from top to bottom from left to right, and recording the number of continuous white line segments of each line; counting from the line with at least 1 white line segment until the whole picture is calculated;

5) judging the change condition of the number of the continuous line segments, if the number of the line segments in the upper line is K more than that of the line segments in the line, K downward convex critical points exist in the upper line; if the number of the line segments in the previous line is less than that of the line segments in the previous line by K, K upwards-convex critical points exist in the line; recording the line number of the critical point and recording the number of the critical point;

6) sequentially taking the rows with the critical points, sequentially taking the critical points of the rows, and filling black to the left pixel by pixel from the critical points until the pixel points to be filled are black; sequentially filling black pixels to the right until the pixel points to be filled are black; processing of the single critical point is finished, and then each critical point is processed according to the mode;

7) and obtaining a final effect graph, wherein each white connected region is a partition, as shown in fig. 1.

The method automatically divides the cost map into a plurality of areas, and effectively decomposes the complex map into small areas with smaller and more single shapes. The difficulty of seamless planning of the map is reduced, multi-machine cooperative operation path planning is facilitated, and the working efficiency and the product applicability are greatly improved. The seamless coverage path planning is realized according to the cost map, compared with other planning methods, the path planned by the method is straighter and more effective, the coverage rate is high, and meanwhile, the intelligent vehicle can track the path planned by the method more easily.