CN116804556B - A probability maximization path planning method for arbitrary convex polygon robots - Google Patents

A probability maximization path planning method for arbitrary convex polygon robots Download PDF

Info

Publication number
CN116804556B
CN116804556B CN202310526472.XA CN202310526472A CN116804556B CN 116804556 B CN116804556 B CN 116804556B CN 202310526472 A CN202310526472 A CN 202310526472A CN 116804556 B CN116804556 B CN 116804556B
Authority
CN
China
Prior art keywords
path
robot
sub
point
basic
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
Application number
CN202310526472.XA
Other languages
Chinese (zh)
Other versions
CN116804556A (en
Inventor
周洋
宛敏红
张春龙
郑涛
李特
原崧育
张泽清
张晶
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Zhejiang Lab
Original Assignee
Zhejiang Lab
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Zhejiang Lab filed Critical Zhejiang Lab
Priority to CN202310526472.XA priority Critical patent/CN116804556B/en
Publication of CN116804556A publication Critical patent/CN116804556A/en
Application granted granted Critical
Publication of CN116804556B publication Critical patent/CN116804556B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Landscapes

  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

The invention discloses a probability maximization path planning method of an arbitrary convex polygon robot. Maximizing the probability of successfully reaching the target point at the time of online path planning, allows the robot to tend to pass through a broader area, thereby providing more options for successfully bypassing obstacles during navigation. The invention maximizes the probability of reaching the target point in each planning, rather than simply searching the shortest path, so that the path planning speed is high and the planning result is excellent.

Description

Probability maximization path planning method for arbitrary convex polygon robot
Technical Field
The invention relates to the field of mobile robots, in particular to a probability maximization path planning method for an arbitrary convex polygon robot.
Background
Mobile robots have been a hotspot in research fields for decades, one of the rapidly developing technologies in the world today, attracting intensive research by scholars of various countries. Technological advances over the years have brought robotics from laboratories to almost all aspects of science, engineering, industry, manufacturing, transportation and life. The mobile robot executes tasks in a complex environment and has the functions of positioning, sensing, planning, controlling and the like, namely, the mobile robot performs sensing modeling on the surrounding environment in the motion process to obtain the pose of the mobile robot, plans the global path and the local track, and controls the robot to move along the expected track. The path planning of the mobile robot means that under a specific environment, an optimal or suboptimal collision-free path from an initial position to a target position is found according to a specific performance index (usually, the shortest path). Path planning techniques can generally be divided into two broad categories: global path planning and local path planning. Based on the determination of the known surrounding environment, the robot needs to perform global path planning, such as Dijkstra algorithm, a-algorithm, visual method, artificial potential field method, etc., before starting navigation. In local path planning, the surrounding environment is usually unknown or almost unknown, and only the surrounding environment information can be obtained from the sensor (such as a laser radar), and some popular algorithms are for example fast exploring random trees (RRT), artificial neural networks, particle Swarm Optimization (PSO), ant colony Algorithm (ACO), etc.
The conventional path planning mainly has the following three problems: 1) The shape of the robot is simply regarded as a circle, and the specific shape is not considered; 2) Only the shortest path can be found, the robot can possibly be guided to pass through a narrow area, and when more static or dynamic obstacles are found due to environmental changes, the possibility of bypassing the obstacles is low; 3) The map needs to be updated continuously according to the laser radar data, and then the route searching is performed based on the updated map in real time, so that the problem of large calculation amount exists.
Disclosure of Invention
Aiming at the defects of the prior art, the invention provides a probability maximization path planning method of an arbitrary convex polygon robot, which belongs to local path planning, is not only applicable to round robots, but also applicable to arbitrary convex polygon robots, generates a basic path library offline, and calculates the collision relation between the convex polygon robot and an obstacle offline. Maximizing the probability of successfully reaching the target point at the time of online path planning, allows the robot to tend to pass through a broader area, thereby providing more options for successfully bypassing obstacles during navigation. As a large amount of calculation is finished off-line, the on-line calculation is reduced as much as possible, and therefore, the path planning method provided by the invention has very high solving speed.
The aim of the invention is achieved by the following technical scheme:
A probability maximization path planning method for an arbitrary convex polygon robot, the method comprising the steps of:
Step one: generating a basic path library covering a 360-degree range around the robot offline, and calculating the collision relation between each basic path in the basic path library and each grid point in the range surrounding the basic path library offline;
step two: acquiring necessary information including a static map, robot motion capability, robot positioning and real-time laser data, searching based on the static map and the laser data to obtain a global path from a starting point to the target point and bypassing an obstacle after obtaining the position of the target point, and smoothing the global path to obtain a global smooth path;
Step three: on the global smooth path, a point with the distance S from the current actual position of the robot is selected as a sub-target point between the current position of the robot and the position of the target point, and a local path with the maximum probability of reaching the sub-target point is planned based on the collision relationship between the basic path obtained in the step one and the grid point; as the robot moves, sub-target points are also dynamically selected on the global path.
Further, the first step includes the following sub-steps:
s101: starting from the starting point, rotating according to a preset length and a preset angle, and generating an end point when rotating once; then generating a spline curve between the starting point and each end point; these multiple spline curves scattered from the starting point form a first scattered sub-path;
S102: performing the same operation as S101 with the end point of each sub-path in the first-time scattered paths as a start point, to obtain second-time scattered sub-paths scattered again from the end point of each first-time scattered sub-path;
S103: and the sub-paths are further scattered by taking the end point of the previous sub-path as a starting point every time, so as to obtain a plurality of scattered sub-paths; the method comprises the steps that any one of the final end points from the initial start point to the corresponding final end point is a basic path, and the basic paths form a basic path library;
S104: calculating the minimum range surrounding all basic paths, uniformly discretizing a continuous curve forming each basic path into a plurality of points, and detecting whether grid points in the minimum range enter the interior of the robot contour when the robot moves along the basic path; if the collision occurs, the collision occurs; otherwise, there is no collision; and storing the established collision relation between the grid points and the basic paths into a relation list.
Further, the preset angles of rotation are equal when the sub-paths are scattered each time, and the number of sub-paths scattered from each starting point is equal; the distances from the start point to the end point are also equal in each of the scattered sub-paths.
Further, the third step comprises the following sub-steps:
S301: expanding peripheral contour points of the obstacle obtained by the laser radar, wherein the expansion size is i;
s302: marking whether each basic path collides with the peripheral outline point of each obstacle according to the collision relation between each grid point and the basic path generated in the off-line in the step one;
S303: taking the basic paths with the first scattered sub-paths overlapped as a group, calculating the probability that each group of collision-free paths reach the current sub-target point, and sequencing the collision-free path groups according to the probability from large to small;
S304: performing in-situ rotation collision detection on the robot;
S305: judging whether a collision-free path group with the highest probability of passing the collision detection is found, if not, reducing the value of the expansion size i, repeating S302-S304 until i is reduced to 0, and indicating that a safety path does not exist; if yes, iteration is terminated, and the sub-path scattered for the first time by the collision-free path group is output as the optimal local path.
Further, in S301, expanding the peripheral contour points of the obstacle obtained by the laser radar specifically includes:
For each data point obtained by the laser radar, taking the data point as a center, respectively adding a plurality of map grid points along the positive and negative directions of x and y parallel to the horizontal plane, and finally forming a plurality of map grid points surrounding the data point; the expansion dimension i is the multiple of the data point after expansion relative to the data point before expansion.
Further, in S303, the probability that each group of collision-free paths reaches the current sub-target point is the sum of the probabilities that all the basic paths in the group reach the sub-target point; the probability of each basic path is calculated by the included angle between the direction of the end of the path and the direction of the sub-target point.
Further, in S304, when performing the robot in-situ rotation collision check, it is checked whether it is safe to rotate from the current direction of the robot to the path direction.
Further, in order to cover a 360 ° range around the robot, the generation operation of the basic path of S101 to S103 is performed once every rotation of a fixed angle around the robot.
An electronic device, comprising:
One or more processors;
and the storage device is used for storing one or more programs, and when the one or more programs are executed by the electronic equipment, the electronic equipment can realize the probability maximization path planning method of the random convex polygon robot.
A computer readable storage medium having stored thereon a program which, when executed by a processor, implements a method for probability maximization path planning for said arbitrary convex polygon robot.
The beneficial effects of the invention are as follows:
1. The path planning method provided by the invention is not only suitable for the circular robot, but also can be applied to any convex polygon robot.
2. The path planning method provided by the invention maximizes the probability of reaching the target point at each planning time instead of simply searching the shortest path.
3. The path planning method provided by the invention reduces the online calculation amount as much as possible, and a large amount of calculation is finished offline. In order to avoid online searching and reduce the computational complexity, obstacles in a certain range near the robot are considered to be known, a basic path library is generated offline, and a collision-free path with the highest probability is selected from the candidate path library only according to obstacle information obtained by a laser radar during online planning.
Drawings
Fig. 1 is a schematic diagram of 7 path groups in an embodiment.
Fig. 2 is a schematic view of the expansion of an obstacle, from left to right, with expansion sizes of 0, 1, 2, 3, respectively.
Fig. 3 is a schematic view of a rectangular robot in-situ rotation security inspection.
Fig. 4 is a test scenario of embodiment 1.
Fig. 5 is a schematic diagram of the actual running path of the rectangular robot in embodiment 1.
Fig. 6 is a schematic diagram of a mobile robot used for the test in example 2.
Fig. 7 is a practical scene of embodiment 2.
Fig. 8 is a schematic diagram of a global smooth path and dynamically selecting sub-target points in embodiment 2.
Fig. 9 is a schematic diagram of a current feasible path and an optimal path of the mobile robot in embodiment 2.
Fig. 10 is a schematic diagram of an actual running path of the mobile robot in embodiment 2.
Detailed Description
Reference will now be made in detail to exemplary embodiments, examples of which are illustrated in the accompanying drawings. When the following description refers to the accompanying drawings, the same numbers in different drawings refer to the same or similar elements, unless otherwise indicated. The implementations described in the following exemplary examples do not represent all implementations consistent with the application. Rather, they are merely examples of apparatus and methods consistent with aspects of the application as detailed in the accompanying claims.
The terminology used herein is for the purpose of describing particular embodiments only and is not intended to be limiting of the application. As used in this specification and the appended claims, the singular forms "a," "an," and "the" are intended to include the plural forms as well, unless the context clearly indicates otherwise. It should also be understood that the term "and/or" as used herein refers to and encompasses any or all possible combinations of one or more of the associated listed items.
It should be understood that although the terms first, second, third, etc. may be used herein to describe various information, these information should not be limited by these terms. These terms are only used to distinguish one type of information from another. For example, first information may also be referred to as second information, and similarly, second information may also be referred to as first information, without departing from the scope of the application. The word "if" as used herein may be interpreted as "at … …" or "at … …" or "in response to a determination" depending on the context.
The probability maximization path planning method of any convex polygon robot is used for avoiding online searching, reducing calculation complexity, regarding an obstacle in a certain range near the robot as known, generating a basic path library offline, and calculating the collision relationship between the convex polygon robot and the obstacle offline. Maximizing the probability of successfully reaching the target point at the time of online path planning, allows the robot to tend to pass through a broader area, thereby providing more options for successfully bypassing obstacles during navigation. In order to enable the robot to keep a certain safety distance from an obstacle in the moving process, an obstacle expansion method is provided, collision detection on a path is not needed on line, and the calculated amount is greatly reduced. As a large amount of calculation is finished off-line, the on-line calculation is reduced as much as possible, and therefore, the path planning method provided by the invention has very high solving speed.
The probability maximization path planning method of any convex polygon robot provided by the embodiment of the invention comprises the following steps:
Step one: generating a basic path library covering a 360-degree range around the robot offline, and calculating the collision relation between each basic path in the basic path library and each grid point in the range surrounding the basic path library offline.
S101: starting from the starting point, rotating according to a preset length and a preset angle, and generating an end point when rotating once; then generating a spline curve between the starting point and each end point; these multiple spline curves scattered from the starting point form a first scattered sub-path;
S102: performing the same operation as S101 with the end point of each sub-path in the first-time scattered paths as a start point, to obtain second-time scattered sub-paths scattered again from the end point of each first-time scattered sub-path;
S103: and the sub-paths are further scattered by taking the end point of the previous sub-path as a starting point every time, so as to obtain a plurality of scattered sub-paths; the basic paths form a basic path library from the initial starting point to any final end point.
Preferably, the preset angle of rotation is equal each time the sub-paths are scattered, and the number of sub-paths scattered from each starting point is equal; the distances from the start point to the end point are also equal in each of the scattered sub-paths.
To cover a 360 ° range around the robot, step S101 is performed every 10 ° range. Fig. 1 shows an example of a basic path library, within 10 degrees, from a starting point, scattered in 7 horizontal directions according to a sub path length of 1 meter, and the scattered angles are-27 degrees, -18 degrees, -9 degrees, 0 degrees, 9 degrees, 18 degrees and 27 degrees, respectively, and each sub path is generated by using a cubic spline curve. These 7 paths are denoted as r 1-r 7 as 7 path groups. Then, the end points of the 7 sub-paths are used as starting points and are dispersed in 7 horizontal directions in the same way to obtain secondary unfolded sub-paths; s101 is performed again, 7 horizontal direction dispersion is performed again. Therefore, each set of paths contains 49 paths, and 7 paths together have 343 paths. These 343 paths are merely the number of paths within 10 degrees around the overlay robot, which contains 252 path groups in total within 360 degrees around the overlay robot.
S104: calculating the minimum range surrounding all basic paths, uniformly discretizing a continuous curve forming each basic path into a plurality of points, and detecting whether grid points in the minimum range enter the interior of the robot contour when the robot moves along the basic path; if the collision occurs, the collision occurs; otherwise, there is no collision; and storing the established collision relation between the grid points and the basic paths into a relation list.
When the robot approximates a circle, it is only necessary to calculate whether the distance between the grid point and each point on the path is smaller than the radius of the robot in order to determine whether the grid point enters the interior of the robot contour. Whereas for any convex polygon robot it has to be checked if a grid point enters the inside of the convex polygon when the polygon robot moves along the path, the calculation of the correspondence will take more time than for a circular robot. Fortunately, it is calculated off-line and does not affect the computational efficiency at on-line run-time. And when the system is started, loading a pre-generated path library and collision relations between the paths and each grid point into a memory of a robot computer.
Step two: necessary information including a static map, robot motion capability, robot positioning and real-time laser data is acquired, after the position of a target point is obtained, a global path from a starting point to the target point and bypassing an obstacle is obtained by searching based on the static map and the laser data, and the global path is smoothed to obtain the global smooth path.
The static map comprises a grid map, map resolution (e.g. 5 cm), map origin coordinates.
Step three: on the global smooth path, a point with the distance S from the current actual position of the robot is selected as a sub-target point between the current position of the robot and the position of the target point, and a local path with the maximum probability of reaching the sub-target point is planned based on the collision relationship between the basic path obtained in the step one and the grid point; as the robot moves, sub-target points are also dynamically selected on the global path.
The third step comprises the following sub-steps:
S301: and expanding the peripheral contour points of the obstacle obtained by the laser radar, wherein the expansion size is i.
As an example, as shown in fig. 2, for each data point obtained by the laser radar, 8 laser points are added to expand, in such a way that i map grid points are respectively expanded in the forward and backward directions parallel to x and y with the laser point as the center, and finally a plurality of map grid points surrounding the data point are formed. When the expansion size is i, it is expressed as i times the size occupied by the data points after expansion relative to the data points before expansion. Fig. 2 gives a schematic illustration of the increase of 8 laser spots with expansion dimensions i=0, 1,2, 3.
S302: and marking whether each basic path collides with the peripheral outline point of each obstacle according to the collision relation between each grid point and the basic path generated in the off-line in the step one.
S303: and taking the basic paths with the first scattered sub-paths overlapped as a group, calculating the probability that each group of collision-free paths reaches the current sub-target point, and sequencing the collision-free path groups according to the probability from large to small.
Preferably, the probability that each group of collision-free paths reaches the current sub-target point is the sum of the probabilities that all basic paths in the group reach the sub-target points; the probability of each basic path is calculated by the included angle between the direction of the end of the path and the direction of the sub-target point.
As one example, all collision-free paths are found from the lidar data. If collision-free paths exist, the probability that all path groups where the collision-free paths are located reach the sub-target points is calculated. Taking path group r1 as an example, the probability of reaching the sub-target point is the sum of 49 paths consisting of r11 to r17, r111 to r 117. The probability magnitude of each path is calculated from the direction of the end of the path and the direction of the target point. And finally, sorting the path groups according to the probability from large to small.
S304: and performing in-situ rotation collision detection on the robot.
If the current orientation of the robot does not coincide with the tangential direction at the start of the selected path, the robot will first rotate in place. As shown in fig. 3, for a circular robot, the robot in-situ rotation must be safe, whereas for a convex polygon robot, it is necessary to check whether it is safe to rotate from the current orientation to the path orientation in-situ. Without loss of generality, the method of in-situ rotational collision detection is exemplified by a rectangular robot, if the obstacle distance d 1 is greater than the radius of the circumscribed circle of the rectangle, the rectangle will not collide, and the maximum allowable angles alpha 1、α2 for clockwise and counterclockwise rotation are 180 degrees respectively. If d 1 is smaller than the inscribed circle radius of the rectangle, then a collision has occurred. If d 1 is in between, then α 1、α2 is calculated. In the example shown in fig. 3, the maximum allowable rotation angle of the robot clockwise is α 1, and the maximum allowable rotation angle of the robot counterclockwise is α 2 =180 degrees ifOr alternativelyThe in-situ rotation is safe.
S305: judging whether a collision-free path group with the highest probability of passing the collision detection is found, if not, reducing the value of the expansion size i, repeating S302-S304 until i is reduced to 0, and indicating that a safety path does not exist; if yes, iteration is terminated, and the sub-path scattered for the first time by the collision-free path group is output as the optimal local path.
The pseudocode for finding the optimal security path is as follows:
the process according to the invention is further illustrated in the following in two examples.
Example 1
Taking autonomous movement of a rectangular robot in a test scene as an example, the embodiment comprises the following specific steps:
step one: offline generating a basic path library and calculating collision relation
A basic path library is generated in advance, and collision relation between paths in the path library and each grid point is calculated offline.
Step two: inputting necessary information and dynamically selecting sub-target points.
And inputting information such as a static map, robot motion capability, robot positioning, target point position, real-time laser data and the like. The test scenario is shown in fig. 4, with dimensions of 10m long and 5m wide. The resolution of the grid map is 5cm, the left lower corner of the map is the origin, the coordinates of the origin are [ -2.0, -2.0], the black part in the grid map indicates that the grid is occupied, the white part indicates that the grid is idle, and no obstacle exists. The laser radar is installed on the robot, so that laser data can be acquired in real time. As shown in fig. 4, the initial position of the robot is directed to the right and is in a stationary state. The motion capabilities of the robot are as follows: linear velocity v e 0,1 m/s, linear acceleration av e-1, 1 m/s 2, angular velocity ω e-1, 1 rad/s, angular acceleration aw e-1, 1 rad/s 2. In this embodiment, the sub-target points are not dynamically selected from the global smooth path, but are fixed, and small circles in fig. 4 represent the sub-target points. The two sides of the rectangular robot at the initial moment are provided with barriers, and the rectangular robot cannot rotate to the target position in situ.
Step three: probability maximization online path planning for arbitrary convex polygon robots
Firstly, expanding laser points (data points obtained by a laser radar are peripheral contour points of an obstacle), wherein the expansion size i is n (n is the maximum expansion size and is a parameter); then, according to the collision relation between the grid points generated offline and the paths, marking whether each path collides with the laser point or not; then calculating the probability that each group of collision-free paths reach the current sub-target point; and finally, performing in-situ rotation collision detection, and finding a path group with the highest probability from the safety paths as an optimal local path. If a safe path with the highest probability is found, the iteration is terminated and the optimal path is output, otherwise, the value of i is reduced, and the steps are repeated. If i is reduced to 0, there is still no safe path, indicating a path planning failure. The online local path planning mainly comprises the following steps:
1) Expanding the laser point;
2) Calculating the probability of the path group reaching the sub-target point;
3) Rotating in situ security inspection;
4) Finding the optimal safe path.
The robot moves from the initial position to the target position autonomously, and on the basis of local path planning, the robot is controlled to move along the planned path by utilizing a path tracking module. Fig. 5 shows an actual path diagram of autonomous movement of a rectangle from an initial state to a target point, indicating that the path planning method proposed by the present invention can be applied to a rectangular robot.
Example 2
The embodiment further describes taking the autonomous movement of the actual mobile robot in the actual scene as an example, and the specific steps are as follows:
step one: offline generating a basic path library and calculating collision relation
A basic path library is generated in advance, and collision relation between paths in the path library and each grid point is calculated offline.
Step two: inputting necessary information, dynamically selecting sub-target point
And inputting information such as a static map, robot motion capability, robot positioning, target point position, real-time laser data and the like. As shown in fig. 6, the robot in actual use is mounted with a 3D laser radar, and distances from the rotation center of the robot to the front, rear, left and right sides are 0.2m, 0.6m, 0.35m and 0.35m, respectively. The motion capabilities of the robot are as follows: linear velocity v e 0,1 m/s, linear acceleration av e-1, 1 m/s 2, angular velocity ω e-1, 1 rad/s, angular acceleration aw e-1, 1 rad/s 2. The actual test scenario is shown in fig. 7, where the start and target points of the navigation task are marked, as well as the added obstacles. The static map built in the scene is shown in fig. 8, the robot is located at the starting point at the initial moment, after receiving the target point, the robot searches the global collision-free path first and smoothes the global collision-free path, and the obtained global smooth path is shown in fig. 8. Points on the global smooth path that are at a distance from the robot position are dynamically selected as sub-target points of the local path planning algorithm, as indicated by small circles in fig. 8.
Step three: probability maximization online path planning for arbitrary convex polygon robots
Mainly comprises the following steps:
1) Expanding the laser point;
2) Calculating the probability of the path group reaching the sub-target point;
3) Rotating in situ security inspection;
4) Finding the optimal safe path.
According to real-time obstacle information obtained by laser, calculating the probability that each path in a basic path library reaches a sub-target point, wherein the probability of a collision-free path is greater than 0, the probability of the collision path is 0, and fig. 9 shows all collision-free paths and optimal safety paths of the robot at the current moment in the motion process, wherein a dotted line is the collision-free path, and a solid line with an arrow is the optimal safety path. On the basis of local path planning, the robot is controlled to move along the planned path by utilizing a path tracking module. The actual running route of the test robot from the start point to the target point is shown in fig. 10.
The electronic device provided by the embodiment of the invention can be applied to any device with data processing capability, and the any device with data processing capability can be a device or a device such as a computer. The apparatus embodiments may be implemented by software, or may be implemented by hardware or a combination of hardware and software. Taking software implementation as an example, the device in a logic sense is formed by reading corresponding computer program instructions in a nonvolatile memory into a memory by a processor of any device with data processing capability. In terms of hardware, in addition to the processor, the memory, the network interface, and the nonvolatile memory, any device with data processing capability in the embodiments of the present invention generally may further include other hardware according to the actual function of the any device with data processing capability, which will not be described herein.
The implementation process of the functions and roles of each unit in the above device is specifically shown in the implementation process of the corresponding steps in the above method, and will not be described herein again.
For the device embodiments, reference is made to the description of the method embodiments for the relevant points, since they essentially correspond to the method embodiments. The apparatus embodiments described above are merely illustrative, wherein the elements illustrated as separate elements may or may not be physically separate, and the elements shown as elements may or may not be physical elements, may be located in one place, or may be distributed over a plurality of network elements. Some or all of the modules may be selected according to actual needs to achieve the purposes of the present invention. Those of ordinary skill in the art will understand and implement the present invention without undue burden.
The embodiment of the invention also provides a computer readable storage medium, on which a program is stored, which when executed by a processor, implements the probability maximization path planning method of any convex polygon robot in the above embodiment.
The computer readable storage medium may be an internal storage unit, such as a hard disk or a memory, of any of the data processing enabled devices described in any of the previous embodiments. The computer readable storage medium may also be an external storage device, such as a plug-in hard disk, a smart memory card (SMARTMEDIA CARD, SMC), an SD card, a flash memory card (FLASH CARD), etc. provided on the device. Further, the computer readable storage medium may include both internal storage units and external storage devices of any data processing device. The computer readable storage medium is used for storing the computer program and other programs and data required by the arbitrary data processing apparatus, and may also be used for temporarily storing data that has been output or is to be output.
It will be appreciated by persons skilled in the art that the foregoing description is a preferred embodiment of the invention, and is not intended to limit the invention, but rather to limit the invention to the specific embodiments described, and that modifications may be made to the technical solutions described in the foregoing embodiments, or equivalents may be substituted for elements thereof, for the purposes of those skilled in the art. Modifications, equivalents, and alternatives falling within the spirit and principles of the invention are intended to be included within the scope of the invention.

Claims (9)

1.一种任意凸多边形机器人的概率最大化路径规划方法,其特征在于,该方法包括如下步骤:1. A probability maximization path planning method for an arbitrary convex polygon robot, characterized in that the method comprises the following steps: 步骤一:离线生成涵盖机器人周围360°范围的基本路径库,离线计算基本路径库中的每条基本路径与包围基本路径库的范围内的每个网格点的碰撞关系;Step 1: Generate a basic path library covering 360° around the robot offline, and calculate the collision relationship between each basic path in the basic path library and each grid point in the range surrounding the basic path library offline; 步骤二:获取包括静态地图、机器人运动能力、机器人定位、实时激光数据在内的必要信息,得到目标点位置后,基于静态地图和激光数据搜索得到一条从起始点到目标点、绕开障碍物的全局路径,并对其进行平滑,得到全局平滑路径;Step 2: Obtain necessary information including static map, robot motion capability, robot positioning, and real-time laser data. After obtaining the location of the target point, search for a global path from the starting point to the target point and bypass obstacles based on the static map and laser data, and smooth it to obtain a global smooth path. 步骤三:在全局平滑路径上,在机器人当前位置和目标点位置之间,选取与机器人当前实际位置的距离为S的点作为子目标点,基于步骤一获得的基本路径与网格点的碰撞关系,规划机器人当前实际位置到达子目标点概率最大的局部路径;随着机器人移动,子目标点也在全局路径上动态选取;Step 3: On the global smooth path, between the current position of the robot and the position of the target point, select a point with a distance S from the current actual position of the robot as a sub-target point. Based on the collision relationship between the basic path obtained in step 1 and the grid points, plan a local path with the highest probability of the robot's current actual position reaching the sub-target point. As the robot moves, the sub-target point is also dynamically selected on the global path. 所述步骤一包括如下子步骤:The step 1 includes the following sub-steps: S101:从起点出发,按照预设的长度、预设的角度进行旋转,每旋转一次,生成一个终点;然后在该起点和每个终点间生成样条曲线;这些从起点散开的多条样条曲线组成第一次散开的子路径;S101: Starting from a starting point, rotating according to a preset length and a preset angle, generating an end point each time the rotation is performed; then generating a spline curve between the starting point and each end point; the multiple spline curves spread out from the starting point constitute a first spread out sub-path; S102:以第一次散开的路径中的每条子路径的终点作为起点,执行和S101相同的操作,得到从每个第一次散开的子路径的终点再次散开的第二次散开的子路径;S102: taking the end point of each sub-path in the first dispersed path as the starting point, performing the same operation as S101, and obtaining a second dispersed sub-path dispersed again from the end point of each first dispersed sub-path; S103:以此类推,每次都以前一次子路径的终点为起点,对子路径进一步散开,获得多次散开的子路径;从最开始的起点到其对应的最终的任意一个终点为一条基本路径,这些基本路径组成基本路径库;S103: Similarly, each time the end point of the previous sub-path is taken as the starting point, the sub-path is further spread out to obtain multiple spread out sub-paths; a basic path is formed from the initial starting point to any corresponding final end point, and these basic paths constitute a basic path library; S104:计算包围所有基本路径的最小范围,将构成每条基本路径的连续曲线均匀离散化为多个点,检测机器人沿着该条基本路径移动时,最小范围内的网格点是否进入机器人轮廓内部;若进入,则发生碰撞;否则,没有碰撞;将建立的网格点与基本路径之间的碰撞关系存储至一个关系列表中。S104: Calculate the minimum range surrounding all basic paths, evenly discretize the continuous curves constituting each basic path into multiple points, and detect whether the grid points within the minimum range enter the robot contour when the robot moves along the basic path; if so, a collision occurs; otherwise, there is no collision; the established collision relationship between the grid points and the basic path is stored in a relationship list. 2.根据权利要求1所述的任意凸多边形机器人的概率最大化路径规划方法,其特征在于,每次子路径散开时旋转的预设角度相等,从每个起点散开的子路径数量相等;每次散开的子路径中,从起点到终点的距离也相等。2. According to the probability maximization path planning method for an arbitrary convex polygon robot according to claim 1, it is characterized in that the preset angle of rotation is equal each time the sub-path is dispersed, and the number of sub-paths dispersed from each starting point is equal; in each dispersed sub-path, the distance from the starting point to the end point is also equal. 3.根据权利要求1所述的任意凸多边形机器人的概率最大化路径规划方法,其特征在于, 所述步骤三包括如下子步骤:3. The probability maximization path planning method for an arbitrary convex polygon robot according to claim 1, characterized in that step three comprises the following sub-steps: S301:对激光雷达获得的障碍物的外围轮廓点进行膨胀,膨胀尺寸为i;S301: dilate the outer contour points of the obstacle obtained by the laser radar, and the dilation size is i; S302:根据步骤一离线生成的每个网格点与基本路径的碰撞关系,标记每条基本路径是否与每个障碍物的外围轮廓点发生碰撞;S302: According to the collision relationship between each grid point and the basic path generated offline in step 1, mark whether each basic path collides with the outer contour point of each obstacle; S303:将第一次散开的子路径重合的基本路径作为一组,计算每一组无碰撞路径到达当前的子目标点的概率,并按照概率从大到小对无碰撞路径组进行排序;S303: taking the basic paths that overlap the sub-paths dispersed for the first time as a group, calculating the probability of each group of collision-free paths reaching the current sub-target point, and sorting the collision-free path groups from large to small according to the probability; S304:进行机器人原地旋转碰撞检查;S304: Performing a collision check on the robot during rotation in situ; S305:判断是否找到通过碰撞检查中的概率最大的无碰撞路径组,若为否,则减小膨胀尺寸i的值,重复S302~S304,直至i减小到0,此时说明不存在安全路径;若为是,则迭代终止,并输出该无碰撞路径组第一次散开的子路径作为最优局部路径。S305: Determine whether the collision-free path group with the highest probability of passing the collision check is found. If not, reduce the value of the expansion size i and repeat S302~S304 until i is reduced to 0, which means that there is no safe path. If yes, the iteration is terminated and the sub-path of the first dispersion of the collision-free path group is output as the optimal local path. 4.根据权利要求3所述的任意凸多边形机器人的概率最大化路径规划方法,其特征在于,S301中,对激光雷达获得的障碍物的外围轮廓点进行膨胀,具体包括:4. The probability maximization path planning method for an arbitrary convex polygon robot according to claim 3 is characterized in that, in S301, the outer contour points of the obstacle obtained by the laser radar are expanded, specifically comprising: 对于每一个激光雷达获得的数据点,以该数据点为中心,沿平行于水平面的x,y正反方向各增加若干个地图栅格点,最后形成包围该数据点的多个地图栅格点;膨胀尺寸i为膨胀后的数据点所占尺寸相对于膨胀前的倍数。For each data point obtained by the laser radar, a number of map grid points are added along the positive and negative directions of x and y parallel to the horizontal plane with the data point as the center, and finally a plurality of map grid points surrounding the data point are formed; the expansion size i is the multiple of the size of the data point after expansion relative to the size before expansion. 5.根据权利要求3所述的任意凸多边形机器人的概率最大化路径规划方法,其特征在于,S303中,每一组无碰撞路径到达当前的子目标点的概率,为该组内所有基本路径到达子目标点的概率之和;而每一条基本路径的概率通过路径末端的朝向与子目标点的方向的夹角大小进行计算。5. The probability maximization path planning method for an arbitrary convex polygon robot according to claim 3 is characterized in that, in S303, the probability of each group of collision-free paths reaching the current sub-target point is the sum of the probabilities of all basic paths in the group reaching the sub-target point; and the probability of each basic path is calculated by the angle between the direction of the path end and the direction of the sub-target point. 6.根据权利要求3所述的任意凸多边形机器人的概率最大化路径规划方法,其特征在于,S304中,进行机器人原地旋转碰撞检查时,需检查从机器人当前朝向原地旋转至路径方向是否安全。6. The probability maximization path planning method for an arbitrary convex polygon robot according to claim 3 is characterized in that, in S304, when performing a collision check for the robot's rotation in situ, it is necessary to check whether it is safe to rotate from the robot's current direction in situ to the path direction. 7.根据权利要求1所述的任意凸多边形机器人的概率最大化路径规划方法,其特征在于,为覆盖机器人周围360°范围,在机器人周围每旋转固定角度,执行一次S101~S103的基本路径的生成操作。7. The probability maximization path planning method for an arbitrary convex polygon robot according to claim 1 is characterized in that, in order to cover a 360° range around the robot, the basic path generation operation of S101 to S103 is performed once every fixed angle rotation around the robot. 8.一种电子设备,其特征在于,包括:8. An electronic device, comprising: 一个或多个处理器;one or more processors; 存储装置,用于存储一个或多个程序,当所述一个或多个程序被所述电子设备执行时,使得所述电子设备实现如权利要求1至7中任一项所述的任意凸多边形机器人的概率最大化路径规划方法。A storage device for storing one or more programs, which, when executed by the electronic device, enables the electronic device to implement the probability maximization path planning method for an arbitrary convex polygon robot as described in any one of claims 1 to 7. 9.一种计算机可读存储介质,其特征在于,其上存储有程序,该程序被处理器执行时,实现权利要求1~7中任一项的所述的任意凸多边形机器人的概率最大化路径规划方法。9. A computer-readable storage medium, characterized in that a program is stored thereon, and when the program is executed by a processor, the probability maximization path planning method of any convex polygon robot described in any one of claims 1 to 7 is implemented.
CN202310526472.XA 2023-05-11 2023-05-11 A probability maximization path planning method for arbitrary convex polygon robots Active CN116804556B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310526472.XA CN116804556B (en) 2023-05-11 2023-05-11 A probability maximization path planning method for arbitrary convex polygon robots

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310526472.XA CN116804556B (en) 2023-05-11 2023-05-11 A probability maximization path planning method for arbitrary convex polygon robots

Publications (2)

Publication Number Publication Date
CN116804556A CN116804556A (en) 2023-09-26
CN116804556B true CN116804556B (en) 2024-07-19

Family

ID=88080131

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310526472.XA Active CN116804556B (en) 2023-05-11 2023-05-11 A probability maximization path planning method for arbitrary convex polygon robots

Country Status (1)

Country Link
CN (1) CN116804556B (en)

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN119573754B (en) * 2024-11-25 2025-09-19 华东交通大学 A method and system for path planning of equipment groups without human intervention based on RRT* algorithm
CN119245660A (en) * 2024-12-02 2025-01-03 中国矿业大学 A path planning optimization method for support handling robot in coal mines

Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108775902A (en) * 2018-07-25 2018-11-09 齐鲁工业大学 The adjoint robot path planning method and system virtually expanded based on barrier
CN115808919A (en) * 2021-09-13 2023-03-17 七海行(深圳)科技有限公司 Method and system for planning path of special-shaped robot

Family Cites Families (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US11530921B2 (en) * 2018-09-28 2022-12-20 Intel Corporation Method of generating a collision free path of travel and computing system
CN111938513B (en) * 2020-06-30 2021-11-09 珠海市一微半导体有限公司 A method, chip and robot for selecting an edge path for robot to overcome obstacles
CN115857516B (en) * 2023-03-02 2023-07-14 之江实验室 Method and device for full-coverage path planning combining cattle-plowing movement and genetic algorithm

Patent Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108775902A (en) * 2018-07-25 2018-11-09 齐鲁工业大学 The adjoint robot path planning method and system virtually expanded based on barrier
CN115808919A (en) * 2021-09-13 2023-03-17 七海行(深圳)科技有限公司 Method and system for planning path of special-shaped robot

Also Published As

Publication number Publication date
CN116804556A (en) 2023-09-26

Similar Documents

Publication Publication Date Title
US12204336B2 (en) Apparatus, method and article to facilitate motion planning in an environment having dynamic objects
Zhang et al. Path planning for indoor mobile robot based on deep learning
Sudhakara et al. Trajectory planning of a mobile robot using enhanced A-star algorithm
CN116804556B (en) A probability maximization path planning method for arbitrary convex polygon robots
Saeedi et al. Group mapping: A topological approach to map merging for multiple robots
CN115562290B (en) A robot path planning method based on A-star penalty control optimization algorithm
Zhao et al. A path planning method based on multi-objective cauchy mutation cat swarm optimization algorithm for navigation system of intelligent patrol car
CN113867347A (en) Robot path planning method, device, management system and computer storage medium
JP2016085689A (en) Autonomous mobile device
CN114200920A (en) Path planning method, device and control system
Saldana et al. A distributed multi-robot approach for the detection and tracking of multiple dynamic anomalies
Ganapathy et al. Fuzzy and neural controllers for acute obstacle avoidance in mobile robot navigation
Wang et al. A Path Planning Algorithm for UAV 3D Surface Inspection Based on Normal Vector Filtering and Integrated Viewpoint Evaluation
Chen et al. An enhanced dynamic Delaunay triangulation-based path planning algorithm for autonomous mobile robot navigation
Xu et al. Hybrid frontier detection strategy for autonomous exploration in multi-obstacles environment
Gao et al. A novel local path planning method considering both robot posture and path smoothness
Jia et al. Portable planner for enhancing ground robots exploration performance in unstructured environments
CN118168546A (en) Collision risk detection method, device, equipment and storage medium
Aryadi et al. Redundant Voronoi Roadmap Graph Using Imaginary Obstacles for Multi-Robot Path Planning
CN117629203A (en) Method, device and equipment for generating chassis docking points in composite robot operations
Omar et al. 3D path planning for unmanned aerial vehicles using visibility line based method
Kala et al. Robotic path planning using multi neuron heuristic search
REN et al. On a Laser Rangefinder-based Mobile Robot Obstacle Avoidance Strategy via Local Virtual Target.
Sánchez et al. Sensor-based probabilistic roadmaps for car-like robots
Herr et al. Cardynet: Deep learning based navigation for car-like robots in dynamic environments

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