CN112504273B - Seamless connection planning method for arcuate path - Google Patents
Seamless connection planning method for arcuate path Download PDFInfo
- Publication number
- CN112504273B CN112504273B CN202011176849.6A CN202011176849A CN112504273B CN 112504273 B CN112504273 B CN 112504273B CN 202011176849 A CN202011176849 A CN 202011176849A CN 112504273 B CN112504273 B CN 112504273B
- Authority
- CN
- China
- Prior art keywords
- point
- line
- area
- taking
- planned
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
Abstract
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.
Description
Technical Field
The invention relates to the technical field of robot navigation obstacle avoidance, in particular to a seamless joint planning method for an arch path.
Background
With the vigorous development of intelligent robot technology, robots are more and more widely applied in production and life. According to different application environments, robots can be divided into various categories, for example, industrial robots often have a plurality of joints and mechanical arms, and the industrial robots are subjected to high-efficiency 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 domestic 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 real-time obstacle avoidance algorithm of the robot, a dynamic cost map is of vital importance, the dynamic cost map shows obstacles detected by a sensor in real time, the information of the obstacles 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: a method for planning the seamless connection of an arch path 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 obstacles and motion 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 terminal point of the area to the starting point of the area to be planned;
7) repeating the steps 4) -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, however, the present invention may be practiced otherwise than as specifically described herein, and it will be appreciated by those skilled in the art that the present invention may be practiced without departing from the spirit and scope of the present invention and that the present invention is not limited by 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 terminal point of the area to the starting point of the area to be planned;
7) repeating the step 4) to the step 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 angle
Wherein x2Is not equal to x1If x is2＝x1Then 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)0；
Angle of rotation theta 0＝(θ1*L1+θ2*L2+θ3*L3+...+θn*Ln)/n (3)
Wherein n is the number of line segments;
3) map rotation-theta using opencv0When 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.
Claims (1)
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 r pixel values of obstacles and movement boundaries in the cost map, 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 path starting point is located to the closer end of the adjacent line segment of the area to be planned, and sequentially connecting all the line segments of 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 terminal point of the area to the starting point of the area to be planned;
7) repeating the steps 4) to 6) until all the areas are connected;
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 angle
Wherein x2Is not equal to x1If x is2＝x1Then 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)0；
Angle of rotation theta0＝(θ1*L1+θ2*L2+θ3*L3+...+θn*Ln)/n (3)
Wherein n is the number of line segments;
3) rotating map by-theta using opencv0When 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.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011176849.6A CN112504273B (en) | 2020-10-29 | 2020-10-29 | Seamless connection planning method for arcuate path |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011176849.6A CN112504273B (en) | 2020-10-29 | 2020-10-29 | Seamless connection planning method for arcuate path |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN112504273A CN112504273A (en) | 2021-03-16 |
| CN112504273B true CN112504273B (en) | 2022-05-24 |
Family
ID=74954411
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202011176849.6A Active CN112504273B (en) | 2020-10-29 | 2020-10-29 | Seamless connection planning method for arcuate path |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN112504273B (en) |
Families Citing this family (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113375674B (en) * | 2021-06-16 | 2024-02-27 | 上海联适导航技术股份有限公司 | Curve path generation method, device, equipment and readable storage medium |
| CN116185045A (en) * | 2023-04-26 | 2023-05-30 | 麦岩智能科技(北京)有限公司 | Path planning method, path planning device, electronic equipment and medium |
Citations (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN104615138A (en) * | 2015-01-14 | 2015-05-13 | 上海物景智能科技有限公司 | Dynamic indoor region coverage division method and device for mobile robot |
| CN107340768A (en) * | 2016-12-29 | 2017-11-10 | 珠海市微半导体有限公司 | A kind of paths planning method of intelligent robot |
| CN107368079A (en) * | 2017-08-31 | 2017-11-21 | 珠海市微半导体有限公司 | Robot cleans the planing method and chip in path |
| CN108896048A (en) * | 2018-06-01 | 2018-11-27 | 浙江亚特电器有限公司 | Paths planning method for mobile carrier |
| CN109947114A (en) * | 2019-04-12 | 2019-06-28 | 南京华捷艾米软件科技有限公司 | Robot complete coverage path planning method, device and equipment based on grating map |
| CN110398964A (en) * | 2019-07-16 | 2019-11-01 | 浙江大学 | A kind of low energy loss robot complete coverage path planning method and system |
| CN110502006A (en) * | 2019-07-22 | 2019-11-26 | 中国矿业大学 | A kind of Mine Abandoned Land mobile robot complete coverage path planning method |
| CN110595478A (en) * | 2019-09-16 | 2019-12-20 | 北京华捷艾米科技有限公司 | Robot full-coverage path planning method, device and equipment based on off-line map |
| CN111543908A (en) * | 2020-05-15 | 2020-08-18 | 弗徕威智能机器人科技（上海）有限公司 | Method and device for planning travelling path and intelligent equipment travelling path |
-
2020
- 2020-10-29 CN CN202011176849.6A patent/CN112504273B/en active Active
Patent Citations (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN104615138A (en) * | 2015-01-14 | 2015-05-13 | 上海物景智能科技有限公司 | Dynamic indoor region coverage division method and device for mobile robot |
| CN107340768A (en) * | 2016-12-29 | 2017-11-10 | 珠海市微半导体有限公司 | A kind of paths planning method of intelligent robot |
| CN107368079A (en) * | 2017-08-31 | 2017-11-21 | 珠海市微半导体有限公司 | Robot cleans the planing method and chip in path |
| CN108896048A (en) * | 2018-06-01 | 2018-11-27 | 浙江亚特电器有限公司 | Paths planning method for mobile carrier |
| CN109947114A (en) * | 2019-04-12 | 2019-06-28 | 南京华捷艾米软件科技有限公司 | Robot complete coverage path planning method, device and equipment based on grating map |
| CN110398964A (en) * | 2019-07-16 | 2019-11-01 | 浙江大学 | A kind of low energy loss robot complete coverage path planning method and system |
| CN110502006A (en) * | 2019-07-22 | 2019-11-26 | 中国矿业大学 | A kind of Mine Abandoned Land mobile robot complete coverage path planning method |
| CN110595478A (en) * | 2019-09-16 | 2019-12-20 | 北京华捷艾米科技有限公司 | Robot full-coverage path planning method, device and equipment based on off-line map |
| CN111543908A (en) * | 2020-05-15 | 2020-08-18 | 弗徕威智能机器人科技（上海）有限公司 | Method and device for planning travelling path and intelligent equipment travelling path |
Also Published As
| Publication number | Publication date |
|---|---|
| CN112504273A (en) | 2021-03-16 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN112504273B (en) | Seamless connection planning method for arcuate path | |
| US10124488B2 (en) | Robot control system and method for planning driving path of robot | |
| Elbanhawi et al. | Randomized bidirectional B-spline parameterization motion planning | |
| Fernandes et al. | Towards an orientation enhanced astar algorithm for robotic navigation | |
| Ali et al. | An algorithm for multi-robot collision-free navigation based on shortest distance | |
| CN110936383A (en) | Obstacle avoiding method, medium, terminal and device for robot | |
| JP5241306B2 (en) | Autonomous mobile device | |
| CN111459160B (en) | Large-scale track smoothing method for unmanned washing and sweeping vehicle on open road | |
| CN110160533B (en) | Path planning method for obstacle avoidance of mobile robot under multi-convex hull obstacle model | |
| CN109794943A (en) | A kind of turning migration path and determining method | |
| CN109634284A (en) | The paths planning method of robot actuating station avoidance based on nested three points of algorithms | |
| US20230063845A1 (en) | Systems and methods for monocular based object detection | |
| Ma et al. | Efficient reciprocal collision avoidance between heterogeneous agents using ctmat | |
| CN113848892B (en) | Robot cleaning area dividing method, path planning method and device | |
| TW201821765A (en) | Method of navigating an automated guided vehicle | |
| CN112286194B (en) | Cost map area division method | |
| CN115223039A (en) | Robot semi-autonomous control method and system for complex environment | |
| JP2006293976A (en) | Autonomous moving device | |
| JP5212939B2 (en) | Autonomous mobile device | |
| CN114995465B (en) | Multi-unmanned vehicle motion planning method and system considering vehicle motion capability | |
| Yang et al. | A smooth jump point search algorithm for mobile robots path planning based on a two-dimensional grid model | |
| Luh | A scheme for collision avoidance with minimum distance traveling for industrial robots | |
| CN113146637B (en) | Robot Cartesian space motion planning method | |
| CN113370816B (en) | Quadruped robot charging pile and fine positioning method thereof | |
| CN115167425A (en) | Map construction and path planning method for quadruped robot |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |