CN114415652B - Path planning method for wheeled robot - Google Patents
Path planning method for wheeled robot Download PDFInfo
- Publication number
- CN114415652B CN114415652B CN202111318627.8A CN202111318627A CN114415652B CN 114415652 B CN114415652 B CN 114415652B CN 202111318627 A CN202111318627 A CN 202111318627A CN 114415652 B CN114415652 B CN 114415652B
- Authority
- CN
- China
- Prior art keywords
- cost
- road
- robot
- axis
- path planning
- 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
- 238000000034 method Methods 0.000 title claims abstract description 32
- 238000010845 search algorithm Methods 0.000 claims description 7
- 238000004364 calculation method Methods 0.000 claims description 6
- 238000010248 power generation Methods 0.000 claims description 3
- 230000009286 beneficial effect Effects 0.000 abstract description 2
- 230000001174 ascending effect Effects 0.000 description 2
- 238000011161 development Methods 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 230000004807 localization Effects 0.000 description 2
- 238000013507 mapping Methods 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000005457 optimization Methods 0.000 description 2
- 238000013473 artificial intelligence Methods 0.000 description 1
- 238000006243 chemical reaction Methods 0.000 description 1
- 238000010276 construction Methods 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 230000000149 penetrating effect Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 230000009466 transformation Effects 0.000 description 1
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
- G05D1/0238—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using obstacle or wall sensors
- G05D1/024—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using obstacle or wall sensors in combination with a laser
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0221—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process
Abstract
The invention belongs to the technical field of robot path planning application, and particularly discloses a wheeled robot path planning method. And 3, calculating the gradient cost among each gradient node of the road surface. And 4, calculating the congestion cost of the road surface in real time. And 5, adding the road gradient cost and the real-time road congestion cost into the total cost of the D algorithm, and carrying out path planning by using the D algorithm. The path planning method for the wheeled robot has the beneficial effects that: 1. the algorithm has short running time, and can find an optimal path faster; 2. the intelligent planning of the path under the complex environment is realized, and the road sections with obstacles and steep slopes can be effectively treated.
Description
Technical Field
The invention belongs to the technical field of robot path planning application, and particularly relates to a wheeled robot path planning method.
Background
With the development of artificial intelligence, the internet of things, cloud computing and other technologies, the development design and construction technology of robots are greatly improved, and robots are increasingly applied to various fields by people.
SLAM (simultaneous localization and mapping), instant localization and mapping and path planning have been central problems in the field of robotic research. Because the environment where the robot is in is complex and changeable in reality, the automatic addressing capability of the robot is greatly influenced by the environment, and the automatic navigation addressing capability is still not ideal.
Accordingly, based on the above-mentioned problems, the present invention provides a path planning method for a wheeled robot.
Disclosure of Invention
The invention aims to: the invention aims to provide a path planning method of a wheeled robot, which aims to realize the identification of the wheeled robot on obstacles, road gradients and road congestion conditions in a complex environment, so that the method can be used for well penetrating the obstacles, avoiding the situation that the power of a steep road is insufficient and avoiding the congestion road, and can be used for quickly and efficiently reaching a destination.
The technical scheme is as follows: the invention discloses a path planning method for a wheeled robot, which comprises the following steps of 1, establishing and generating a three-dimensional point cloud map, and 2, generating a three-dimensional grid map containing road congestion, obstacle size and road gradient information according to the three-dimensional point cloud map. And 3, calculating the gradient cost among each gradient node of the road surface. And 4, calculating the congestion cost of the road surface in real time. And 5, adding the road gradient cost and the real-time road congestion cost into the total cost of the D algorithm, and carrying out path planning by using the D algorithm.
According to the technical scheme, in the step 3, whether the robot can pass through the obstacle or not is judged according to the size of the robot, and the three-dimensional grid map is updated; for an uphill road section, a sub-item of the cost function additional information item A (n) is searched according to the robot weight, the road friction coefficient and the maximum power generation D of the robot.
According to the technical scheme, in the step 4, a sub-item of the additional information item A (n) of the D-path search cost function is generated according to the road congestion condition.
In the technical scheme, in the step 5, an additional information item a (n) of the cost function is added to a cost function F (n) in a D-path search algorithm to form a total cost function F (n) =g (n) +h (n) +a (n); and D, carrying out path optimization by taking the cost total function F (n) as a basis.
In the technical scheme, in the step 5, an additional information item a (n) of a cost function of a path search algorithm is added, so that the additional information item a (n) becomes: f (n) =g (n) +h (n) +a (n), wherein G (n) is the cost from the target node to any node n, H (n) is the heuristic cost from node n to the starting point, and the additional information a (n) includes road congestion condition cost information, obstacle size cost information and road gradient cost information, and a path is planned by using a D-path search algorithm.
According to the technical scheme, in the step 3, for an ascending road section, ascending resistance f=mu (mgCosθ) +mgSinθ is calculated according to the weight of the robot, wherein g is a proportionality coefficient, and the size is about 9.8N/kg, and the gradient of a θ road surface is calculated; in the step 4, as for the congestion information contained in the map, kH (n) is set as a sub-item of the additional information item a (n), k is a congestion coefficient of the road surface, the range of the road surface congestion coefficient k is more than or equal to 0 and less than or equal to 1, k=0 is smooth road surface, and k=1 is severe congestion of the road surface.
According to the technical scheme, θ is a gradient angle formed by the adjacent node and the ground plane, and a calculation formula is as follows:
X a 、X b 、X c is the coordinates of the X axis, the Y axis and the Z axis of the point A, Y a 、Y b 、Y c The coordinates of the point B are x-axis, y-axis and z-axis.
Cost of additional information item in the technical proposalD is the maximum power which can be output by the robot, and the cost of the additional information item A (n) is added into the D total cost function to obtain a new costIs equal to F (n) =g (n) +h (n) +a (n).
In the technical scheme, if the power system of the robot is insufficient to provide power larger than the uphill resistance f, the cost of the additional information item A (n) is set to be infinite, otherwise, one sub item is set to be
According to the technical scheme, H (n) is Euler distance, and a calculation formula is as follows:
compared with the prior art, the path planning method for the wheeled robot has the beneficial effects that: 1. the algorithm has short running time, and the optimal path can be found out faster; 2. the intelligent planning of the path under the complex environment is realized, and the road sections with obstacles and steep slopes can be effectively treated.
Drawings
Fig. 1 is an algorithm flow chart of a path planning method of a wheeled robot of the present invention.
Detailed Description
The invention is further elucidated below in connection with the drawings and the specific embodiments.
The invention discloses a path planning method for a wheeled robot, which comprises the following steps of 1, establishing and generating a three-dimensional point cloud map, and 2, generating a three-dimensional grid map containing road congestion, obstacle size and road gradient information according to the three-dimensional point cloud map. And 3, calculating the gradient cost among each gradient node of the road surface. And 4, calculating the congestion cost of the road surface in real time. And 5, adding the road gradient cost and the real-time road congestion cost into the total cost of the D algorithm, and carrying out path planning by using the D algorithm.
According to the wheeled robot path planning method, in the step 3, whether the robot can pass through an obstacle or not is judged according to the size of the robot, and a three-dimensional grid map is updated; for an uphill road section, a sub-item of the cost function additional information item A (n) is searched according to the robot weight, the road friction coefficient and the maximum power generation D of the robot.
According to the path planning method of the wheeled robot, in the step 4, the sub-item of the additional information item A (n) of the D-path search cost function is generated according to the road congestion condition.
In the path planning method of the wheeled robot, in the step 5, an additional information item A (n) of the cost function is added into a cost function F (n) in a path searching algorithm D to form a total cost function F (n) =G (n) +H (n) +A (n); and D, carrying out path optimization by taking the cost total function F (n) as a basis.
Further, in step 5, the additional information item a (n) of the cost function of the path search algorithm is added to make it become: f (n) =g (n) +h (n) +a (n), wherein G (n) is the cost from the target node to any node n, H (n) is the heuristic cost from node n to the starting point, and the additional information a (n) includes road congestion condition cost information, obstacle size cost information and road gradient cost information, and a path is planned by using a D-path search algorithm.
Further, in the step 3, for the uphill road section, calculating an uphill resistance f=μ (mgCos θ) +mgsin θ according to the weight of the robot, wherein g is a proportionality coefficient, and the magnitude is about 9.8N/kg, and the θ road surface gradient; in the step 4, as for the congestion information contained in the map, kH (n) is set as a sub-item of the additional information item a (n), k is a congestion coefficient of the road surface, the range of the road surface congestion coefficient k is more than or equal to 0 and less than or equal to 1, k=0 is smooth road surface, and k=1 is severe congestion of the road surface.
According to the path planning method of the wheeled robot, θ is a gradient angle formed by adjacent nodes and a ground plane, and a calculation formula is as follows:
X a 、X b 、X c an x axis of the A point,Y-axis, z-axis coordinates, Y a 、Y b 、Y c The coordinates of the point B are x-axis, y-axis and z-axis.
The invention relates to a path planning method of a wheeled robot, which is added with cost of information items D is the maximum power that the robot can output, and the additional information item a (n) cost is added into the D total cost function, so as to obtain a new total cost function F (n) =g (n) +h (n) +a (n).
The wheel type robot path planning method of the invention sets the cost of the additional information item A (n) as infinity if the power system of the robot is insufficient to provide power larger than the uphill resistance f, otherwise sets one of the additional information items as
The path planning method of the wheeled robot, H (n) is Euler distance, and the calculation formula is as follows:
as shown in fig. 1, the three-dimensional grid map is first built to include the size of the obstacle, such as the size of the side of the table, so as to compare with the size of the robot itself, thereby judging whether the robot can pass through the obstacle; secondly, if the robot finds that a new obstacle exists on the path during movement, a new three-dimensional grid map is required to be scanned again; and finally, road congestion cost is mainly applied to a scene that a plurality of robots work simultaneously, so that the situation that the robot and other robots squeeze the road is prevented, and the road congestion cost needs to be calculated in real time by a separate process.
Examples
A robot path planning method comprises the following steps:
1) The wheeled robot is provided with a laser radar and a depth camera, a plurality of methods for establishing a three-dimensional point cloud MAP can be adopted, and SLAM methods such as RTAB-MAP and the like can be adopted;
2) In the embodiment, the coordinates of the point cloud world coordinate system are calculated through the projection model, and the three-dimensional rigid transformation principle is adopted to obtain the conversion relationship between the depth image and the point cloud coordinates as follows:
z＝d
where x, y, z are three-dimensional coordinates in the camera coordinate system, u, v are the positions (rows and columns) of pixels in the image, c x And c y Is the coordinates of the camera optics in the image coordinate system, c if the camera is not distorted x And c y W/2 and H/2, respectively, where W and H are the width and height of the image, respectively. f (f) x And f y The focal lengths of the cameras in the x-axis and the y-axis, respectively.
3) And adding the road congestion condition, the obstacle size information into the three-dimensional grid map to form the three-dimensional grid map with the road congestion condition, the obstacle size information and the road gradient information.
4) Establishing a priority queue (OpenList) by taking a target point as a starting point, and placing the target point in the OpenList;
5) The route points in the map are represented by states, and each State contains a pointer (backhaul) to the previous State; the State Tag of the current State has New, open, closed three states, new indicates that the State is not placed in an OpenList, openindicates that the State is being placed in the OpenList, and CLosed indicates that the State is taken out from the OpenList;
6) Placing all adjacent nodes (except for obstacles and unreachable nodes) of the target point into a priority queue (OpenList), calculating the cost from the adjacent point of each target point to the target point and the estimated cost from the adjacent point of each target point to the robot position node, and calculating the node with the minimum cost from the adjacent point to the target node in the OpenList;
7) Removing the node with the minimum estimated cost from the OpenList table to the target node, and putting the neighbor nodes of the node into the OpenList table;
8) Repeating steps 6 and 7 until the State of the node where the robot is located is Closed or the OPenList table is empty (indicating that there is no path to the target node)
9) If the robot detects that the environment changes in the moving process, the method enters step 3 to restart searching.
The foregoing is merely a preferred embodiment of the invention, and it should be noted that modifications could be made by those skilled in the art without departing from the principles of the invention, which modifications would also be considered to be within the scope of the invention.
Claims (7)
1. A path planning method for a wheeled robot is characterized in that: comprises the steps of,
step 1, establishing and generating a three-dimensional point cloud map;
step 2, generating a three-dimensional grid map containing road congestion, obstacle size and road gradient information according to the three-dimensional point cloud map;
step 3, calculating gradient cost among gradient nodes of the road surface;
step 4, calculating the congestion cost of the road surface in real time;
step 5, adding the road gradient cost and the real-time road congestion cost into the total cost of the D-algorithm, and carrying out path planning by using the D-algorithm;
in the step 3, for the uphill road section, calculating an uphill resistance f=μ (mgCos θ) +mgsin θ according to the weight of the robot, wherein u is a resistance coefficient, m is the weight of the robot, g is a proportionality coefficient, and the size is about 9.8N/kg, θ road gradient; in the step 4, as for the congestion information contained in the map, kH (n) is set as a sub-item of the additional information item A (n), k is the congestion coefficient of the road surface, the range of the road surface congestion coefficient k is more than or equal to 0 and less than or equal to 1, k=0 is the road surface unblocked, and k=1 is the road surface severe congestion; in the step 5, an additional information item a (n) of the cost function of the path search algorithm is added to be changed into: f (n) =g (n) +h (n) +a (n), wherein G (n) is the cost from the target node to any node n, H (n) is the heuristic cost from node n to the starting point, and the additional information a (n) includes road congestion condition cost information, obstacle size cost information and road gradient cost information, and a path is planned by using a D-path search algorithm.
2. A wheeled robotic path planning method according to claim 1, wherein: in the step 3, judging whether the robot can pass through the obstacle according to the size of the robot, and updating the three-dimensional grid map; for an uphill road section, a sub-item of the cost function additional information item A (n) is searched according to the robot weight, the road friction coefficient and the maximum power generation D of the robot.
3. A wheeled robotic path planning method according to claim 1, wherein: and in the step 4, generating a sub-item of the additional information item A (n) of the D path search cost function according to the road congestion condition.
4. A wheeled robotic path planning method according to claim 1, wherein: θ is a gradient angle formed by the adjacent node and the ground plane, and the calculation formula is as follows:
X a 、Y a 、Z a is the coordinates of the X axis, the Y axis and the Z axis of the point A, Y b 、Y b 、Z b The coordinates of the point B are x-axis, y-axis and z-axis.
5. A wheeled robotic path planning method according to claim 1, wherein: cost of additional information itemD is the maximum power that the robot can output, and the additional information item a (n) cost is added into the D total cost function, so as to obtain a new total cost function F (n) =g (n) +h (n) +a (n).
6. A wheeled robotic path planning method according to claim 1, wherein: if the robot power system is insufficient to provide power greater than the uphill resistance f, setting the cost of the additional information item A (n) to infinity, otherwise setting one of the additional information items to be
7. A wheeled robotic path planning method according to claim 5, wherein: h (n) is Euler distance, and the calculation formula is:X a 、Y a 、Z a is the coordinates of the X axis, the Y axis and the Z axis of the point A, Y b 、Y b 、Z b The coordinates of the point B are x-axis, y-axis and z-axis.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111318627.8A CN114415652B (en) | 2021-11-09 | 2021-11-09 | Path planning method for wheeled robot |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111318627.8A CN114415652B (en) | 2021-11-09 | 2021-11-09 | Path planning method for wheeled robot |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN114415652A CN114415652A (en) | 2022-04-29 |
| CN114415652B true CN114415652B (en) | 2024-03-26 |
Family
ID=81265145
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202111318627.8A Active CN114415652B (en) | 2021-11-09 | 2021-11-09 | Path planning method for wheeled robot |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114415652B (en) |
Citations (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107526360A (en) * | 2017-09-26 | 2017-12-29 | 河南科技学院 | The multistage independent navigation detection system of explosive-removal robot and method under a kind of circumstances not known |
| CN108088445A (en) * | 2016-11-22 | 2018-05-29 | 广州映博智能科技有限公司 | 3 d grid map path planning system and method based on octree representation |
| CN109798909A (en) * | 2019-02-01 | 2019-05-24 | 安徽达特智能科技有限公司 | A kind of method of global path planning |
| CN111046846A (en) * | 2019-12-27 | 2020-04-21 | 长安大学 | Method for judging passing ability of obstacle in front of robot |
| CN111830986A (en) * | 2020-07-24 | 2020-10-27 | 北京理工大学 | Three-dimensional path planning method for biped robot |
| CN112034836A (en) * | 2020-07-16 | 2020-12-04 | 北京信息科技大学 | Mobile robot path planning method for improving A-x algorithm |
| CN112572416A (en) * | 2020-11-20 | 2021-03-30 | 芜湖格陆博智能科技有限公司 | Parking method and system for unmanned vehicle in mining area |
| CN112650234A (en) * | 2020-12-16 | 2021-04-13 | 浙江大学 | Path planning method of biped robot |
| CN113485346A (en) * | 2021-07-15 | 2021-10-08 | 上海交通大学 | Autonomous navigation method of mobile robot in nuclear accident complex environment |
-
2021
- 2021-11-09 CN CN202111318627.8A patent/CN114415652B/en active Active
Patent Citations (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108088445A (en) * | 2016-11-22 | 2018-05-29 | 广州映博智能科技有限公司 | 3 d grid map path planning system and method based on octree representation |
| CN107526360A (en) * | 2017-09-26 | 2017-12-29 | 河南科技学院 | The multistage independent navigation detection system of explosive-removal robot and method under a kind of circumstances not known |
| CN109798909A (en) * | 2019-02-01 | 2019-05-24 | 安徽达特智能科技有限公司 | A kind of method of global path planning |
| CN111046846A (en) * | 2019-12-27 | 2020-04-21 | 长安大学 | Method for judging passing ability of obstacle in front of robot |
| CN112034836A (en) * | 2020-07-16 | 2020-12-04 | 北京信息科技大学 | Mobile robot path planning method for improving A-x algorithm |
| CN111830986A (en) * | 2020-07-24 | 2020-10-27 | 北京理工大学 | Three-dimensional path planning method for biped robot |
| CN112572416A (en) * | 2020-11-20 | 2021-03-30 | 芜湖格陆博智能科技有限公司 | Parking method and system for unmanned vehicle in mining area |
| CN112650234A (en) * | 2020-12-16 | 2021-04-13 | 浙江大学 | Path planning method of biped robot |
| CN113485346A (en) * | 2021-07-15 | 2021-10-08 | 上海交通大学 | Autonomous navigation method of mobile robot in nuclear accident complex environment |
Also Published As
| Publication number | Publication date |
|---|---|
| CN114415652A (en) | 2022-04-29 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111210518B (en) | Topological map generation method based on visual fusion landmark | |
| CN112859859B (en) | Dynamic grid map updating method based on three-dimensional obstacle object pixel object mapping | |
| CN106371445B (en) | A kind of unmanned vehicle planning control method based on topological map | |
| Ziegler et al. | Spatiotemporal state lattices for fast trajectory planning in dynamic on-road driving scenarios | |
| CN112378408A (en) | Path planning method for realizing real-time obstacle avoidance of wheeled mobile robot | |
| CN110262518A (en) | Automobile navigation method, system and medium based on track topological map and avoidance | |
| CN110531770A (en) | One kind being based on improved RRT paths planning method and system | |
| CN101739509B (en) | Path navigation method for large-scale virtual crowd | |
| CN106017472A (en) | Global path planning method, global path planning system and unmanned aerial vehicle | |
| CN109974739B (en) | Global navigation system based on high-precision map and navigation information generation method | |
| WO2021237667A1 (en) | Dense height map construction method suitable for legged robot planning | |
| Jaspers et al. | Multi-modal local terrain maps from vision and lidar | |
| CN113821029A (en) | Path planning method, device, equipment and storage medium | |
| CN111307158A (en) | AUV three-dimensional route planning method | |
| KR20220083975A (en) | Trajectory planning methods, devices, instruments, storage media and program products | |
| CN110209171A (en) | A kind of paths planning method based on Artificial Potential Field Method | |
| CN114415652B (en) | Path planning method for wheeled robot | |
| CN116136417B (en) | Unmanned vehicle local path planning method facing off-road environment | |
| CN115576333B (en) | Optimal obstacle avoidance strategy | |
| Short et al. | Abio-inspiredalgorithminimage-based pathplanning and localization using visual features and maps | |
| CN117036374A (en) | Laser radar point cloud segmentation and motion planning method for automatic driving | |
| CN114564048A (en) | Improved method for planning flight path of agricultural four-rotor unmanned aerial vehicle | |
| CN114509085A (en) | Quick path searching method combining grid and topological map | |
| Du et al. | Hierarchical path planning and obstacle avoidance control for unmanned surface vehicle | |
| CN114510061A (en) | Unmanned ship movement path planning method and device, terminal equipment and storage medium |
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 |