CN112650242A - Mobile robot path planning method based on hybrid algorithm - Google Patents
Mobile robot path planning method based on hybrid algorithm Download PDFInfo
- Publication number
- CN112650242A CN112650242A CN202011524397.6A CN202011524397A CN112650242A CN 112650242 A CN112650242 A CN 112650242A CN 202011524397 A CN202011524397 A CN 202011524397A CN 112650242 A CN112650242 A CN 112650242A
- Authority
- CN
- China
- Prior art keywords
- mobile robot
- node
- path
- target
- target points
- 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.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 47
- 230000003068 static effect Effects 0.000 claims abstract description 4
- 238000011156 evaluation Methods 0.000 claims description 42
- 239000003016 pheromone Substances 0.000 claims description 29
- 241000257303 Hymenoptera Species 0.000 claims description 25
- 238000005070 sampling Methods 0.000 claims description 18
- 230000008569 process Effects 0.000 claims description 14
- 230000004927 fusion Effects 0.000 claims description 11
- 230000001133 acceleration Effects 0.000 claims description 9
- 238000004088 simulation Methods 0.000 claims description 7
- 230000004888 barrier function Effects 0.000 claims description 6
- 238000012805 post-processing Methods 0.000 claims description 6
- 230000007613 environmental effect Effects 0.000 claims description 4
- 238000010606 normalization Methods 0.000 claims description 4
- 238000012545 processing Methods 0.000 claims description 3
- 230000007704 transition Effects 0.000 claims description 3
- 238000011161 development Methods 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 230000002431 foraging effect Effects 0.000 description 2
- 238000011160 research Methods 0.000 description 2
- 238000007689 inspection Methods 0.000 description 1
Images
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/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0217—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory in accordance with energy consumption, time reduction or distance reduction criteria
Abstract
A mobile robot path planning method based on a hybrid algorithm is a path planning method combining global static planning and local dynamic planning, and the method adopts an A-Star algorithm to obtain a path between two target points, calculates the distance of each path, can plan a global optimal path which starts from a starting point and finally returns to an original point after executing a plurality of tasks, sets sub target points on the basis of the global optimal path, and adopts a dynamic window algorithm to realize local dynamic obstacle avoidance according to the path condition, so that the robot can not only walk along the shortest path, but also carry out real-time obstacle avoidance, and the spherical mobile robot can finish the whole path planning.
Description
The technical field is as follows:
the invention belongs to the field of mobile robots, and particularly relates to a mobile robot path planning method based on a hybrid algorithm.
(II) background technology:
at present, mobile robots are widely used due to their advantages of strong environmental adaptability, high flexibility in movement, etc., including multi-target cruise, search and rescue, survey and exploration, pipeline inspection, resource development, etc. Path planning research of mobile robots has become one of the important research subjects in current mobile robot technology. And the development of path planning also provides safety guarantee for the mobile robot to run to a practical, safer and more reliable task execution. Most of the existing mobile robots can only carry out path planning on a single target point, cannot carry out real-time obstacle avoidance, and cannot meet the requirements of the current society, and if a path of a plurality of target points can be found and the obstacle avoidance can be carried out in real time, the method is particularly important.
(III) the invention content:
the invention aims to provide a mobile robot path planning method based on a hybrid algorithm, which solves the problem that the prior art can only plan a single target point path and also solves the problem of real-time obstacle avoidance, and is a path planning method which is simple to operate and easy to realize.
The technical scheme of the invention is as follows: a mobile robot path planning method based on a hybrid algorithm is characterized by comprising the following steps:
(1) acquiring global environment information through a mobile robot configuration sensor to obtain an environment image signal capable of describing operation environment information;
the mobile robot in the step (1) can be a land robot, an underwater robot or an aerial robot; the mobile robot is provided with a sensor, and the sensor is used for acquiring the global environment information, namely acquiring the global environment information by using an environment information acquisition device and a data acquisition method carried by the mobile robot and constructing an environment image signal capable of describing the operation environment information.
(2) Creating a working environment model of the mobile robot according to the environment image signals collected in the step (1) by using a grid method;
(3) drawing a path of every two target points by utilizing an A-Star algorithm based on the working environment model of the mobile robot created in the step (2);
the planning path between every two target points is planned by adopting an A-Star algorithm in the step (3), and the method comprises the following steps:
(3-1) selecting a starting point of a path to be planned, determining a node to be examined according to an A-Star algorithm principle, and establishing an Open list for storing the node to be examined; selecting a node with the lowest observed and evaluated function from the Open list and putting the node into a Closed list;
the nodes to be examined in the step (3-1) include two situations, namely, the nodes which are adjacent to the nodes around the starting point and take the starting point as the center, and the nodes which are selected from the nodes to be examined and have the lowest evaluation function, and the nodes which take the node with the lowest evaluation function as the center and are adjacent to the nodes around the node; the nodes to be examined include nodes that have already been examined or nodes that contain obstacles.
(3-2) determining a starting grid node S, and putting the starting grid node S into an Open list;
(3-3) selecting in an Open list, calculating an evaluation function of each node to be inspected in the Open list according to the evaluation function shown in the formula (1), and selecting the node with the lowest evaluation function from the evaluation functions to be put into a Closed list;
F(i)=G(i)+H(i) (1)
wherein i represents the ith node, G (i) represents the cost value paid from the start grid node S to the ith node, and H (i) represents the predicted cost value from the ith node to the target point T;
(3-4) if the Open list is empty at the moment, namely the current node is taken as the center, the nodes to be examined in the Open list are divided into two categories, one category is that the barrier grids cannot pass through, and the other category is that the nodes to be examined are already put into the Closed list, so that the nodes to be examined cannot be selected, at the moment, the Open list is empty under both conditions, and the path planning is failed; if the Open list is not empty, searching a node with the minimum evaluation function F (i) in the Open list, taking the node as a current node A, and putting the node A into a Closed list;
(3-5) judging whether the node A selected in the step (3-4) is a target point, if so, ending the process; otherwise, the node A is taken as a father node to expand the child nodes around the node A, and the expansion post-processing is carried out;
the step (3-5) of performing extended post-processing on the child nodes specifically includes:
(3-5-1) if the child node to be expanded is not in the Closed list, the Open list or the barrier grid, adding the node into the Open list;
(3-5-2) if the child node to be expanded is in the Closed list or in the obstacle grid, skipping the child node;
(3-5-3) if the child node already exists in the Open list, judging whether the evaluation function F (i) value of the child node is smaller than the evaluation function F (i) value in the Open list, if so, updating the evaluation function F (i) value in the Open list, traversing the child nodes to be expanded accordingly, and finding the child node with the minimum evaluation function F (i) value.
(3-6) continuing the child node expansion according to the step (3-5) until the target node is put into the Closed list, stopping searching, and showing that a passing path from the starting point to the target point is found; when the Open list is empty and the target node is not found, the search is also stopped, and no path which can pass through is marked.
(4) On the basis of the step (3), determining the traversal order of all the target points by using a fusion ant colony algorithm, wherein a path formed by the traversal order is the shortest distance capable of traversing all the target points:
in the step (4), the traversal order of all the target points is determined by using the ant colony fusion algorithm, and a path formed by the traversal order is the shortest distance capable of traversing all the target points, and the method specifically comprises the following steps:
(4-1) initializing pheromones of all paths of ant colony parameters, wherein the number of ants is m, n is the number of target points, and each ant randomly selects one target point of the n target points as a starting point;
(4-2) each ant calculates the next target point to be visited according to the state transition probability shown in the formula (4), namely, the next target point to be visited is moved from the ith target point to the next vertex j until all ants finish one cycle trip, namely, all the target points are traversed by all the ants;
in the formula, S is an initial grid node; allowedkThe target points which can be moved by the ants in the next step are collected;for the pheromone on the path at time t,is the reciprocal of the path length, alpha is the pheromone importance degree factor, beta is the heuristic function importance degree factor;
(4-3) after all ants complete one cycle, recording the total length L of the path passed by each ant and the shortest path of the current iteration, and updating the pheromone concentration between the target points according to the pheromone updating formulas of formulas (5) to (7):
where ρ is0Is the pheromone volatilization coefficient; n is a radical ofcIteration times of the current ants; n is a radical ofmaxMaximum number of iterations for ants, Δ τi,jThe sum of the concentration values of pheromones from all ants between target points i and j, L is the sum of the traveling distances of the kth ant in the current search, and Q represents the intensity of the pheromones;
and (4-4) if the iteration times reach the maximum, outputting an optimal traversal sequence, taking the optimal traversal sequence as the sequence of the target points traversed by the mobile robot in the path planning process, wherein the path formed by the sequence of the target points traversed and the origin is the shortest distance, otherwise, adding 1 to the iteration times, emptying the ant path recording table, and returning to the step (4-2) to continue executing until the multi-target point global static path planning of the mobile robot is completed.
(5) And (4) moving the mobile robot from the starting point along the path which is found in the step (4) and can traverse the shortest distance of all target points, predicting the motion track of the mobile robot by using local dynamic programming based on a fusion dynamic window algorithm, carrying out real-time obstacle avoidance on the mobile obstacle encountered in the process of moving, selecting an optimal path, enabling the mobile robot to avoid the obstacle, moving along the original path after obstacle avoidance, and finally returning to the starting point.
The step (5) of predicting the motion trajectory of the mobile robot by using the local dynamic programming based on the fusion dynamic window algorithm specifically comprises the following steps:
(5-1) when the mobile robot obtains an instruction for starting to move, based on the path which is globally planned in the step (4), placing the mobile robot at the origin, setting a middle turning point from the initial position to the first target point as a first dynamic sub-target point, and if no turning point exists, setting the first target point as a first dynamic sub-target point;
(5-2) establishing a motion model of the mobile robot: let the linear velocity of the mobile robot be vtAnd an angular velocity of wt(ii) a The pose of the current mobile robot is expressed as (x)0,y0,θ0) The pose at the next moment is recorded as (x)t,yt,θt) Then, the motion model is shown as formula (8):
wherein x is0Abscissa, y, representing the current position of the mobile robot0Ordinate, θ, representing the current position of the mobile robot0Course angle, x, representing the current position of the mobile robottIs the abscissa, y, of the mobile robot at the next momenttIs the ordinate, theta, of the mobile robot at the next momenttRepresenting the course angle of the mobile robot at the next moment;
(5-3) sampling the running speed of the mobile robot on the basis of the motion model of the mobile robot shown in the formula (8), calculating the motion track of the mobile robot according to the sampling speed, and evaluating the track;
the sampling of the running speed of the mobile robot in the step (5-3) is limited by the influence of the self condition and environmental factors of the mobile robot:
(5-3-1) the mobile robot is limited by the maximum and minimum speed of the mobile robot as shown in the formula (9):
vm∈{v∈[vmin,vmax],w∈[wmin,wmax]} (9)
in the formula, vminAt minimum linear velocity, vmaxAt maximum linear velocity, wminIs the minimum angular velocity of rotation, wmaxIs the maximum rotational angular velocity;
(5-3-2) the mobile robot is affected by the performance of its own motor:
because the motor torque is limited and the maximum acceleration and deceleration limit exists, a dynamic window exists in the forward simulation period of the track of the mobile robot, and the speed in the window is the speed which can be actually reached by the mobile robot, namely:
in the formula, vc,wcIs the current velocity and rotational angular velocity of the mobile robot,it is indicated that the instantaneous deceleration is,which represents the instantaneous rate of deceleration of the rotation,which is indicative of the instantaneous acceleration of the vehicle,representing instantaneous rotational acceleration;
(5-3-3) in view of safety of the mobile robot, in order to stop the mobile robot before the mobile robot collides with an obstacle, the traveling of the mobile robot should satisfy a speed range shown in equation (11) under the condition of the maximum deceleration thereof:
where dist (v, w) is the distance on the trajectory corresponding to velocity (v, w) that is closest to the obstacle.
(5-4) obtaining an intersection of the sampling speeds under each condition obtained according to the formulas (9) to (11) in the step (5-3), and obtaining at least one group of speeds; the motion trail of the mobile robot is obtained by substituting the motion trail into a kinematic model shown in an expression (8); different motion tracks can be obtained at different speeds;
(5-5) since no less than 1 possible motion trajectory can be obtained according to the sampling speed group in the step (5-4), each trajectory is evaluated by adopting an evaluation function as shown in the formula (12):
G(v,w)=σ(α·heading(v,w)+β·dist(v,w)+γ·velocity(v,w)) (12)
wherein, the heading (v, w) is used for evaluating the angle difference between the orientation when the mobile robot reaches the tail end of the simulation track and the target at the current set sampling speed; dist (v, w) represents the distance between the mobile robot and the nearest obstacle on the current trajectory; if there is no obstacle on this trajectory, dist (v, w) is set to a constant; velocity (v, w) is used for evaluating the speed of the current track;
(5-6) normalizing the three parts of header (v, w), dist (v, w) and velocity (v, w) in the formula (12) according to the formulas (13) to (15):
(5-7) carrying the header (v, w), dist (v, w) and dist (v, w) subjected to normalization processing in the step (5-6) into a formula (12), selecting the track with the highest evaluation function as the final motion track of the robot, and driving the robot to move according to the speed corresponding to the track;
(5-8) the mobile robot moves forward according to the speed corresponding to the final motion track obtained in the step (5-7) until meeting a target point, and whether the position of the current target point is the position of the starting point is judged, if yes, the mobile robot is the final target point, the motion is finished, otherwise, the mobile robot is ready to move to the next target point, and the steps (5-1) -the step (5-8) are repeated until the robot moves to the starting point, and the process is finished.
The invention has the advantages that: by combining the traditional A-Star algorithm search strategy, the ant colony algorithm is fused to solve the problem of the traversal sequence of a plurality of target points, and the dynamic window algorithm is fused to solve the problem of real-time obstacle avoidance of the mobile robot. The invention mainly solves the problem of real-time obstacle avoidance, because the number of target points is large, a path can be planned in the system after the mobile robot collects the environment, the mobile robot is still at the original point, when the mobile robot moves, the mobile robot possibly touches a moving obstacle to occupy the original path, so that the mobile robot can not complete the task, and after the dynamic obstacle avoidance is added, the mobile robot can bypass the moving obstacle and continue to walk along the original path, thereby better completing the task.
Description of the drawings:
fig. 1 is an algorithm flowchart of a mobile robot path planning method based on a hybrid algorithm according to the present invention.
Fig. 2 is a graph for solving path planning comparison between multiple target points by using a fused ant colony algorithm of the mobile robot path planning method based on the hybrid algorithm according to the present invention (where fig. 2-a is a conventional a-x algorithm, and fig. 2-b is a fused ant colony algorithm).
The specific implementation mode is as follows:
example (b): a mobile robot path planning method based on a hybrid algorithm is shown in figure 1 and is characterized by comprising the following steps:
(1) acquiring global environment information through a mobile robot configuration sensor to obtain an environment image signal capable of describing operation environment information;
wherein the mobile robot is one of a land robot, an underwater robot or an aerial robot; and the environment information acquisition device and the data acquisition method carried by the mobile robot obtain the global environment information and are used for constructing an environment image signal capable of describing the operation environment information. In the embodiment, the robot is a spherical amphibious mobile robot.
(2) Creating a working environment model of the mobile robot according to the environment image signals collected in the step (1) by using a grid method;
(3) drawing a path of every two target points by utilizing an A-Star algorithm based on the working environment model of the mobile robot created in the step (2);
the planning path between every two target points is planned by adopting an A-Star algorithm in the step (3), and the method comprises the following steps:
(3-1) selecting a starting point of a path to be planned, determining a node to be examined according to an A-Star algorithm principle, and establishing an Open list for storing the node to be examined; selecting a node with the lowest observed and evaluated function from the Open list and putting the node into a Closed list;
the nodes to be examined include two conditions, namely, the nodes which are adjacent to the nodes around the starting point from the starting point, and the nodes which are selected from the nodes to be examined and have the lowest evaluation function, and the nodes which are adjacent to the nodes around the nodes with the lowest evaluation function as the center; the nodes to be examined include nodes that have already been examined or nodes that contain obstacles.
(3-2) determining a starting grid node S, and putting the starting grid node S into an Open list;
(3-3) selecting in an Open list, calculating an evaluation function of each node to be inspected in the Open list according to the evaluation function shown in the formula (1), and selecting the node with the lowest evaluation function from the evaluation functions to be put into a Closed list;
F(i)=G(i)+H(i) (1)
wherein i represents the ith node, G (i) represents the cost value paid from the start grid node S to the ith node, and H (i) represents the predicted cost value from the ith node to the target point T;
(3-4) if the Open list is empty at the moment, namely the current node is taken as the center, the nodes to be examined in the Open list are divided into two categories, one category is that the barrier grids cannot pass through, and the other category is that the nodes to be examined are already put into the Closed list, so that the nodes to be examined cannot be selected, at the moment, the Open list is empty under both conditions, and the path planning is failed; if the Open list is not empty, searching a node with the minimum evaluation function F (i) in the Open list, taking the node as a current node A, and putting the node A into a Closed list;
(3-5) judging whether the node A selected in the step (3-4) is a target point, if so, ending the process; otherwise, the node A is taken as a father node to expand the child nodes around the node A, and the expansion post-processing is carried out;
the step (3-5) of performing extended post-processing on the child nodes specifically includes:
(3-5-1) if the child node to be expanded is not in the Closed list, the Open list or the barrier grid, adding the node into the Open list;
(3-5-2) if the child node to be expanded is in the Closed list or in the obstacle grid, skipping the child node;
(3-5-3) if the child node already exists in the Open list, judging whether the evaluation function F (i) value of the child node is smaller than the evaluation function F (i) value in the Open list, if so, updating the evaluation function F (i) value in the Open list, traversing the child nodes to be expanded accordingly, and finding the child node with the minimum evaluation function F (i) value.
(3-6) continuing the child node expansion according to the step (3-5) until the target node is put into the Closed list, stopping searching, and showing that a passing path from the starting point to the target point is found; when the Open list is empty and the target node is not found, the search is also stopped, and no path which can pass through is marked.
(4) On the basis of the step (3), determining the traversal order of all the target points by using a fusion ant colony algorithm, wherein a path formed by the traversal order is the shortest distance capable of traversing all the target points:
the ant colony algorithm is a method for simulating ants to determine foraging paths by using pheromones; and preferentially selecting a path with higher pheromone concentration in the foraging process, releasing quantitative pheromones, forming positive feedback along with the higher and higher concentration of the pheromones, and selecting the path with the highest pheromone concentration as an optimal path by traversing all paths. The ant colony algorithm is applied to solve the TSP problem, the ant colony algorithm can be described as the distance of all the target points is known, and a shortest path traversing n target points is solved on the premise that each target point passes through once. Namely:
since the ant colony algorithm can be described as: knowing the distances between all the target points, a shortest path traversing n target points can be solved on the premise that each target point passes through once, that is:
P=(c1,c2,c3,...cn) (2)
wherein P is the traversal order representation of the target point, ciNumber of target points, i ═ 1,2, … …, n, d (c)i,ci+1) Representing the distance between adjacent numbered target points, f (p) the path through the n target points.
Determining the traversal order of all target points by using a fusion ant colony algorithm, wherein a path formed by the traversal order is the shortest distance capable of traversing all the target points, and the method specifically comprises the following steps:
(4-1) initializing pheromones of all paths of ant colony parameters, wherein the number of ants is m, n is the number of target points, and each ant randomly selects one target point of the n target points as a starting point;
(4-2) each ant calculates the next target point to be visited according to the state transition probability shown in the formula (4), namely, the next target point to be visited is moved from the ith target point to the next vertex j until all ants finish one cycle trip, namely, all the target points are traversed by all the ants;
in the formula, S is an initial grid node; allowedkThe target points which can be moved by the ants in the next step are collected;for the pheromone on the path at time t,is the reciprocal of the path length, alpha is the pheromone importance degree factor, beta is the heuristic function importance degree factor;
(4-3) after all ants complete one cycle, recording the total length L of the path passed by each ant and the shortest path of the current iteration, and updating the pheromone concentration between the target points according to the pheromone updating formulas of formulas (5) to (7):
where ρ is0Is the pheromone volatilization coefficient; n is a radical ofcIteration times of the current ants; n is a radical ofmaxMaximum number of iterations for ants, Δ τi,jThe sum of the concentration values of pheromones from all ants between target points i and j, L is the sum of the traveling distances of the kth ant in the current search, and Q represents the intensity of the pheromones;
and (4-4) if the iteration times reach the maximum, outputting an optimal traversal sequence, taking the optimal traversal sequence as the sequence of the target points traversed by the mobile robot in the path planning process, wherein the path formed by the sequence of the target points traversed and the origin is the shortest distance, otherwise, adding 1 to the iteration times, emptying the ant path recording table, and returning to the step (4-2) to continue executing until the multi-target point global static path planning of the mobile robot is completed.
And (3) finding the shortest distance between every two target points, and (4) finding the best traversal sequence by considering all the points including the starting point and the target points, connecting all the points, wherein the final path lengths are different in different traversal sequences. The sequence of the target points traversed by the ant colony algorithm is the shortest distance, and the distance is the shortest distance connecting all the target points and the starting point.
After the step (4) is completed, the mobile robot finds a shortest distance traversing all target points under the condition of a known environment, but the mobile robot does not start moving yet, the step (5) moves along the path planned in the step (4), and in the moving process, the situation that the path planned originally cannot pass due to the fact that a moving obstacle occupies the path planned already, the moving obstacle needs to be avoided, and the moving robot continues to move along the path planned originally is found.
(5) And (4) moving the mobile robot from the starting point along the path which is found in the step (4) and can traverse the shortest distance of all target points, predicting the motion track of the mobile robot by using local dynamic programming based on a fusion dynamic window algorithm, carrying out real-time obstacle avoidance on the mobile obstacle encountered in the process of moving, selecting an optimal path, enabling the mobile robot to avoid the obstacle, moving along the original path after obstacle avoidance, and finally returning to the starting point.
The method for predicting the motion trail of the mobile robot by utilizing the local dynamic programming based on the fusion dynamic window algorithm specifically comprises the following steps:
(5-1) when the mobile robot obtains an instruction for starting to move, based on the path which is globally planned in the step (4), placing the mobile robot at the origin, setting a middle turning point from the initial position to the first target point as a first dynamic sub-target point, and if no turning point exists, setting the first target point as a first dynamic sub-target point;
(5-2) establishing a motion model of the mobile robot: let the linear velocity of the mobile robot be vtAnd an angular velocity of wt(ii) a The pose of the current mobile robot is expressed as (x)0,y0,θ0) The pose at the next moment is recorded as (x)t,yt,θt) Then, the motion model is shown as formula (8):
wherein x is0Abscissa, y, representing the current position of the mobile robot0Ordinate, θ, representing the current position of the mobile robot0Course angle, x, representing the current position of the mobile robottIs the abscissa, y, of the mobile robot at the next momenttIs the ordinate, theta, of the mobile robot at the next momenttRepresenting the course angle of the mobile robot at the next moment;
(5-3) sampling the running speed of the mobile robot on the basis of the motion model of the mobile robot shown in the formula (8), calculating the motion track of the mobile robot according to the sampling speed, and evaluating the track;
the sampling of the running speed of the mobile robot is limited by the influence of the self condition and environmental factors of the mobile robot:
(5-3-1) the mobile robot is limited by the maximum and minimum speed of the mobile robot as shown in the formula (9):
vm∈{v∈[vmin,vmax],w∈[wmin,wmax]} (9)
in the formula, vminAt minimum linear velocity, vmaxAt maximum linear velocity, wminIs the minimum angular velocity of rotation, wmaxIs the maximum rotational angular velocity;
(5-3-2) the mobile robot is affected by the performance of its own motor:
because the motor torque is limited and the maximum acceleration and deceleration limit exists, a dynamic window exists in the forward simulation period of the track of the mobile robot, and the speed in the window is the speed which can be actually reached by the mobile robot, namely:
in the formula, vc,wcIs the current velocity and rotational angular velocity of the mobile robot,it is indicated that the instantaneous deceleration is,which represents the instantaneous rate of deceleration of the rotation,which is indicative of the instantaneous acceleration of the vehicle,representing instantaneous rotational acceleration;
(5-3-3) in view of safety of the mobile robot, in order to stop the mobile robot before the mobile robot collides with an obstacle, the traveling of the mobile robot should satisfy a speed range shown in equation (11) under the condition of the maximum deceleration thereof:
where dist (v, w) is the distance on the trajectory corresponding to velocity (v, w) that is closest to the obstacle.
(5-4) obtaining an intersection of the sampling speeds under each condition obtained according to the formulas (9) to (11) in the step (5-3), and obtaining at least one group of speeds; the motion trail of the mobile robot is obtained by substituting the motion trail into a kinematic model shown in an expression (8); different motion tracks can be obtained at different speeds;
the speeds after intersection are taken are usually multiple groups, because the speeds in the formulas (9) to (11) are all in one interval, and the speeds are still in one interval after intersection is taken, but the intervals become smaller;
(5-5) since no less than 1 possible motion trajectory can be obtained according to the sampling speed group in the step (5-4), each trajectory is evaluated by adopting an evaluation function as shown in the formula (12):
G(v,w)=σ(α·heading(v,w)+β·dist(v,w)+γ·velocity(v,w)) (12)
wherein, the heading (v, w) is used for evaluating the angle difference between the orientation when the mobile robot reaches the tail end of the simulation track and the target at the current set sampling speed; dist (v, w) represents the distance between the mobile robot and the nearest obstacle on the current trajectory; if there is no obstacle on this trajectory, dist (v, w) is set to a constant; velocity (v, w) is used for evaluating the speed of the current track;
(5-6) normalizing the three parts of header (v, w), dist (v, w) and velocity (v, w) in the formula (12) according to the formulas (13) to (15):
(5-7) carrying the header (v, w), dist (v, w) and dist (v, w) subjected to normalization processing in the step (5-6) into a formula (12), selecting the track with the highest evaluation function as the final motion track of the robot, and driving the robot to move according to the speed corresponding to the track;
the purpose of normalization is to smooth: such as the obstacle distance, the minimum obstacle distance detected by the mobile robot sensor is discontinuous in two-dimensional space, the trajectory can meet the obstacle, the next one can not meet the trajectory, and the minimum obstacle distance of the trajectory is 1m and the next one is 10m, then the evaluation criterion of the obstacle distance results in the discontinuity of the evaluation function, and also results in a term being too dominant in the evaluation function, so they are normalized and become the same percentage, and the minimum distance of each obstacle is one of 100.
(5-8) the mobile robot moves forward according to the speed corresponding to the final motion track obtained in the step (5-7) until meeting a target point, and whether the position of the current target point is the position of the starting point is judged, if yes, the mobile robot is the final target point, the motion is finished, otherwise, the mobile robot is ready to move to the next target point, and the steps (5-1) -the step (5-8) are repeated until the robot moves to the starting point, and the process is finished.
In fig. 2, simulation is performed by using simulation software, the same background environment is selected for path planning in the graph (a) and the graph (b), seven target points are selected in the graph (a) for separate path planning, if a task of each target point is to be completed, the total path length is twice the distance from a single target point to a starting point, and then summation is performed, and the traversal of all the target points is completed in the graph (b) under the same background and the same target point, and the path of the starting point is traversed only once, so that the path length is greatly shortened.
Claims (8)
1. A mobile robot path planning method based on a hybrid algorithm is characterized by comprising the following steps:
(1) acquiring global environment information through a mobile robot configuration sensor to obtain an environment image signal capable of describing operation environment information;
(2) creating a working environment model of the mobile robot according to the environment image signals collected in the step (1) by using a grid method;
(3) drawing a path of every two target points by utilizing an A-Star algorithm based on the working environment model of the mobile robot created in the step (2);
(4) on the basis of the step (3), determining the traversal order of all the target points by using a fusion ant colony algorithm, wherein a path formed by the traversal order is the shortest distance capable of traversing all the target points:
(5) and (4) moving the mobile robot from the starting point along the path which is found in the step (4) and can traverse the shortest distance of all target points, predicting the motion track of the mobile robot by using local dynamic programming based on a fusion dynamic window algorithm, carrying out real-time obstacle avoidance on the mobile obstacle encountered in the process of moving, selecting an optimal path, enabling the mobile robot to avoid the obstacle, moving along the original path after obstacle avoidance, and finally returning to the starting point.
2. The hybrid algorithm-based mobile robot path planning method according to claim 1, wherein the mobile robot in step (1) can be a land robot, an underwater robot or an aerial robot; the mobile robot is provided with a sensor, and the sensor is used for acquiring the global environment information, namely acquiring the global environment information by using an environment information acquisition device and a data acquisition method carried by the mobile robot and constructing an environment image signal capable of describing the operation environment information.
3. The method for planning the path of the mobile robot based on the hybrid algorithm according to claim 1, wherein the step (3) adopts an a-Star algorithm to plan the planned path between each two target points, and comprises the following steps:
(3-1) selecting a starting point of a path to be planned, determining a node to be examined according to an A-Star algorithm principle, and establishing an Open list for storing the node to be examined; selecting a node with the lowest observed and evaluated function from the Open list and putting the node into a Closed list;
(3-2) determining a starting grid node S, and putting the starting grid node S into an Open list;
(3-3) selecting in an Open list, calculating an evaluation function of each node to be inspected in the Open list according to the evaluation function shown in the formula (1), and selecting the node with the lowest evaluation function from the evaluation functions to be put into a Closed list;
F(i)=G(i)+H(i) (1)
wherein i represents the ith node, G (i) represents the cost value paid from the start grid node S to the ith node, and H (i) represents the predicted cost value from the ith node to the target point T;
(3-4) if the Open list is empty at the moment, namely the current node is taken as the center, the nodes to be examined in the Open list are divided into two categories, one category is that the barrier grids cannot pass through, and the other category is that the nodes to be examined are already put into the Closed list, so that the nodes to be examined cannot be selected, at the moment, the Open list is empty under both conditions, and the path planning is failed; if the Open list is not empty, searching a node with the minimum evaluation function F (i) in the Open list, taking the node as a current node A, and putting the node A into a Closed list;
(3-5) judging whether the node A selected in the step (3-4) is a target point, if so, ending the process; otherwise, the node A is taken as a father node to expand the child nodes around the node A, and the expansion post-processing is carried out;
(3-6) continuing the child node expansion according to the step (3-5) until the target node is put into the Closed list, stopping searching, and showing that a passing path from the starting point to the target point is found; when the Open list is empty and the target node is not found, the search is also stopped, and no path which can pass through is marked.
4. The method for planning a path of a mobile robot according to claim 3, wherein the nodes to be examined in step (3-1) include two cases, namely, starting from the starting point, with the starting point as the center and the nodes adjacent to the starting point as the center, and selecting the node with the lowest evaluation function from the nodes to be examined, and then centering on the node with the lowest evaluation function and the nodes adjacent to the node; the nodes to be examined include nodes that have already been examined or nodes that contain obstacles.
5. The method for planning a path of a mobile robot based on a hybrid algorithm according to claim 3, wherein the step (3-5) of performing extended post-processing on the child nodes specifically includes:
(3-5-1) if the child node to be expanded is not in the Closed list, the Open list or the barrier grid, adding the node into the Open list;
(3-5-2) if the child node to be expanded is in the Closed list or in the obstacle grid, skipping the child node;
(3-5-3) if the child node already exists in the Open list, judging whether the evaluation function F (i) value of the child node is smaller than the evaluation function F (i) value in the Open list, if so, updating the evaluation function F (i) value in the Open list, traversing the child nodes to be expanded accordingly, and finding the child node with the minimum evaluation function F (i) value.
6. The method for planning a path of a mobile robot based on a hybrid algorithm according to claim 1, wherein the step (4) determines the traversal order of all the target points by using the fused ant colony algorithm, and the path formed by the traversal order is a shortest distance that can traverse all the target points, which comprises the following steps:
(4-1) initializing pheromones of all paths of ant colony parameters, wherein the number of ants is m, n is the number of target points, and each ant randomly selects one target point of the n target points as a starting point;
(4-2) each ant calculates the next target point to be visited according to the state transition probability shown in the formula (4), namely, the next target point to be visited is moved from the ith target point to the next vertex j until all ants finish one cycle trip, namely, all the target points are traversed by all the ants;
in the formula, S is an initial grid node; allowedkThe target points which can be moved by the ants in the next step are collected;for the pheromone on the path at time t,is the reciprocal of the path length, alpha is the pheromone importance degree factor, beta is the heuristic function importance degree factor;
(4-3) after all ants complete one cycle, recording the total length L of the path passed by each ant and the shortest path of the current iteration, and updating the pheromone concentration between the target points according to the pheromone updating formulas of formulas (5) to (7):
where ρ is0Is the pheromone volatilization coefficient; n is a radical ofcIteration times of the current ants; n is a radical ofmaxMaximum number of iterations for ants, Δ τi,jPheromones from all ants between target points i, jThe sum of concentration values, L is the sum of the distances traveled by the kth ant in the current search, and Q represents the intensity of the pheromone;
and (4-4) if the iteration times reach the maximum, outputting an optimal traversal sequence, taking the optimal traversal sequence as the sequence of the target points traversed by the mobile robot in the path planning process, wherein the path formed by the sequence of the target points traversed and the origin is the shortest distance, otherwise, adding 1 to the iteration times, emptying the ant path recording table, and returning to the step (4-2) to continue executing until the multi-target point global static path planning of the mobile robot is completed.
7. The method for planning the path of the mobile robot based on the hybrid algorithm according to claim 1, wherein the step (5) of predicting the motion trajectory of the mobile robot by using the local dynamic planning based on the fusion dynamic window algorithm specifically comprises the following steps:
(5-1) when the mobile robot obtains an instruction for starting to move, based on the path which is globally planned in the step (4), placing the mobile robot at the origin, setting a middle turning point from the initial position to the first target point as a first dynamic sub-target point, and if no turning point exists, setting the first target point as a first dynamic sub-target point;
(5-2) establishing a motion model of the mobile robot: let the linear velocity of the mobile robot be vtAnd an angular velocity of wt(ii) a The pose of the current mobile robot is expressed as (x)0,y0,θ0) The pose at the next moment is recorded as (x)t,yt,θt) Then, the motion model is shown as formula (8):
wherein x is0Abscissa, y, representing the current position of the mobile robot0Ordinate, θ, representing the current position of the mobile robot0Course angle, x, representing the current position of the mobile robottIs the abscissa, y, of the mobile robot at the next momenttIs the ordinate, theta, of the mobile robot at the next momenttRepresenting the course angle of the mobile robot at the next moment;
(5-3) sampling the running speed of the mobile robot on the basis of the motion model of the mobile robot shown in the formula (8), calculating the motion track of the mobile robot according to the sampling speed, and evaluating the track;
wherein, sampling the running speed of the mobile robot is limited by the influence of the self condition and environmental factors of the mobile robot:
(5-3-1) the mobile robot is limited by the maximum and minimum speed of the mobile robot as shown in the formula (9):
vm∈{v∈[vmin,vmax],w∈[wmin,wmax]} (9)
in the formula, vminAt minimum linear velocity, vmaxAt maximum linear velocity, wminIs the minimum angular velocity of rotation, wmaxIs the maximum rotational angular velocity;
(5-3-2) the mobile robot is affected by the performance of its own motor:
because the motor torque is limited and the maximum acceleration and deceleration limit exists, a dynamic window exists in the forward simulation period of the track of the mobile robot, and the speed in the window is the speed which can be actually reached by the mobile robot, namely:
in the formula, vc,wcIs the current velocity and rotational angular velocity of the mobile robot,it is indicated that the instantaneous deceleration is,which represents the instantaneous rate of deceleration of the rotation,which is indicative of the instantaneous acceleration of the vehicle,representing instantaneous rotational acceleration;
(5-3-3) in view of safety of the mobile robot, in order to stop the mobile robot before the mobile robot collides with an obstacle, the traveling of the mobile robot should satisfy a speed range shown in equation (11) under the condition of the maximum deceleration thereof:
wherein dist (v, w) is the distance on the trajectory corresponding to the velocity (v, w) closest to the obstacle;
(5-4) obtaining an intersection of the sampling speeds under each condition obtained according to the formulas (9) to (11) in the step (5-3), and obtaining at least one group of speeds; the motion trail of the mobile robot is obtained by substituting the motion trail into a kinematic model shown in an expression (8); different motion tracks can be obtained at different speeds;
(5-5) since no less than 1 possible motion trajectory can be obtained according to the sampling speed group in the step (5-4), each trajectory is evaluated by adopting an evaluation function as shown in the formula (12):
G(v,w)=σ(α·heading(v,w)+β·dist(v,w)+γ·velocity(v,w)) (12)
wherein, the heading (v, w) is used for evaluating the angle difference between the orientation when the mobile robot reaches the tail end of the simulation track and the target at the current set sampling speed; dist (v, w) represents the distance between the mobile robot and the nearest obstacle on the current trajectory; if there is no obstacle on this trajectory, dist (v, w) is set to a constant; velocity (v, w) is used for evaluating the speed of the current track;
(5-6) normalizing the three parts of header (v, w), dist (v, w) and velocity (v, w) in the formula (12) according to the formulas (13) to (15):
(5-7) carrying the header (v, w), dist (v, w) and dist (v, w) subjected to normalization processing in the step (5-6) into a formula (12), selecting the track with the highest evaluation function as the final motion track of the mobile robot, and driving the robot to move according to the speed corresponding to the track;
(5-8) the mobile robot moves forward according to the speed corresponding to the final motion track obtained in the step (5-7) until meeting a target point, and whether the position of the current target point is the position of the starting point is judged, if yes, the mobile robot is the final target point, the motion is finished, otherwise, the mobile robot is ready to move to the next target point, and the steps (5-1) -the step (5-8) are repeated until the robot moves to the starting point, and the process is finished.
8. The hybrid algorithm-based mobile robot path planning method according to claim 7, wherein the mobile robot is a spherical amphibious mobile robot.
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202011524397.6A CN112650242A (en) | 2020-12-22 | 2020-12-22 | Mobile robot path planning method based on hybrid algorithm |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202011524397.6A CN112650242A (en) | 2020-12-22 | 2020-12-22 | Mobile robot path planning method based on hybrid algorithm |
Publications (1)
Publication Number | Publication Date |
---|---|
CN112650242A true CN112650242A (en) | 2021-04-13 |
Family
ID=75358751
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202011524397.6A Pending CN112650242A (en) | 2020-12-22 | 2020-12-22 | Mobile robot path planning method based on hybrid algorithm |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN112650242A (en) |
Cited By (12)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN113341991A (en) * | 2021-06-18 | 2021-09-03 | 重庆大学 | Path optimization method based on dynamic window and redundant node filtering |
CN113359808A (en) * | 2021-07-21 | 2021-09-07 | 国网浙江省电力有限公司双创中心 | Multi-stage path planning method for power inspection of unmanned aerial vehicle and related device |
CN113359757A (en) * | 2021-06-30 | 2021-09-07 | 湖北汽车工业学院 | Improved hybrid A-x algorithm unmanned vehicle path planning and trajectory tracking method |
CN113496065A (en) * | 2021-06-29 | 2021-10-12 | 西北工业大学 | Rapid and high-precision network area dynamic coverage track generation method |
CN113835428A (en) * | 2021-08-27 | 2021-12-24 | 华东交通大学 | Robot path planning method for restaurant |
CN114047762A (en) * | 2021-11-15 | 2022-02-15 | 西安工业大学 | Laser radar space measurement path planning method and system based on ant colony algorithm |
CN114199271A (en) * | 2021-12-14 | 2022-03-18 | 浙江大学 | Method for accessing multiple target points through shortest path by using rope-hanging mobile robot |
CN114578798A (en) * | 2022-02-24 | 2022-06-03 | 苏州驾驶宝智能科技有限公司 | Autonomous driving system of air-ground amphibious aerodyne |
CN114770512A (en) * | 2022-05-09 | 2022-07-22 | 浙江大学 | Optimal time planning method for carrying mechanical arm of mobile robot for rescuing and obstacle clearing |
CN115560774A (en) * | 2022-10-24 | 2023-01-03 | 重庆邮电大学 | Mobile robot path planning method oriented to dynamic environment |
CN115990327A (en) * | 2023-03-24 | 2023-04-21 | 建研防火科技有限公司 | Intelligent fire control management system based on thing networking |
CN116011762A (en) * | 2022-12-30 | 2023-04-25 | 珠海市格努信息技术有限公司 | Intelligent scheduling method and device for a plurality of warehouse unmanned vehicles |
Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109947098A (en) * | 2019-03-06 | 2019-06-28 | 天津理工大学 | A kind of distance priority optimal route selection method based on machine learning strategy |
CN110160546A (en) * | 2019-05-10 | 2019-08-23 | 安徽工程大学 | A kind of method for planning path for mobile robot |
CN110675307A (en) * | 2019-08-19 | 2020-01-10 | 杭州电子科技大学 | Implementation method of 3D sparse point cloud to 2D grid map based on VSLAM |
CN111174798A (en) * | 2020-01-17 | 2020-05-19 | 长安大学 | Foot type robot path planning method |
-
2020
- 2020-12-22 CN CN202011524397.6A patent/CN112650242A/en active Pending
Patent Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109947098A (en) * | 2019-03-06 | 2019-06-28 | 天津理工大学 | A kind of distance priority optimal route selection method based on machine learning strategy |
CN110160546A (en) * | 2019-05-10 | 2019-08-23 | 安徽工程大学 | A kind of method for planning path for mobile robot |
CN110675307A (en) * | 2019-08-19 | 2020-01-10 | 杭州电子科技大学 | Implementation method of 3D sparse point cloud to 2D grid map based on VSLAM |
CN111174798A (en) * | 2020-01-17 | 2020-05-19 | 长安大学 | Foot type robot path planning method |
Non-Patent Citations (1)
Title |
---|
黄辰: "基于智能优化算法的移动机器人路径规划与定位方法研究", 《中国优秀博硕士学位论文全文数据库(博士)》 * |
Cited By (18)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN113341991A (en) * | 2021-06-18 | 2021-09-03 | 重庆大学 | Path optimization method based on dynamic window and redundant node filtering |
CN113341991B (en) * | 2021-06-18 | 2022-08-09 | 重庆大学 | Path optimization method based on dynamic window and redundant node filtering |
CN113496065B (en) * | 2021-06-29 | 2023-04-18 | 西北工业大学 | Rapid and high-precision network area dynamic coverage track generation method |
CN113496065A (en) * | 2021-06-29 | 2021-10-12 | 西北工业大学 | Rapid and high-precision network area dynamic coverage track generation method |
CN113359757A (en) * | 2021-06-30 | 2021-09-07 | 湖北汽车工业学院 | Improved hybrid A-x algorithm unmanned vehicle path planning and trajectory tracking method |
CN113359808A (en) * | 2021-07-21 | 2021-09-07 | 国网浙江省电力有限公司双创中心 | Multi-stage path planning method for power inspection of unmanned aerial vehicle and related device |
CN113835428A (en) * | 2021-08-27 | 2021-12-24 | 华东交通大学 | Robot path planning method for restaurant |
CN114047762A (en) * | 2021-11-15 | 2022-02-15 | 西安工业大学 | Laser radar space measurement path planning method and system based on ant colony algorithm |
CN114199271A (en) * | 2021-12-14 | 2022-03-18 | 浙江大学 | Method for accessing multiple target points through shortest path by using rope-hanging mobile robot |
CN114199271B (en) * | 2021-12-14 | 2023-12-26 | 浙江大学 | Method for accessing multiple target points through shortest path by using rope-hanging mobile robot |
CN114578798A (en) * | 2022-02-24 | 2022-06-03 | 苏州驾驶宝智能科技有限公司 | Autonomous driving system of air-ground amphibious aerodyne |
CN114770512A (en) * | 2022-05-09 | 2022-07-22 | 浙江大学 | Optimal time planning method for carrying mechanical arm of mobile robot for rescuing and obstacle clearing |
CN114770512B (en) * | 2022-05-09 | 2023-12-29 | 浙江大学 | Optimal time planning method for carrying mobile robot mechanical arm for rescue obstacle clearance |
CN115560774B (en) * | 2022-10-24 | 2023-11-17 | 重庆邮电大学 | Dynamic environment-oriented mobile robot path planning method |
CN115560774A (en) * | 2022-10-24 | 2023-01-03 | 重庆邮电大学 | Mobile robot path planning method oriented to dynamic environment |
CN116011762A (en) * | 2022-12-30 | 2023-04-25 | 珠海市格努信息技术有限公司 | Intelligent scheduling method and device for a plurality of warehouse unmanned vehicles |
CN115990327A (en) * | 2023-03-24 | 2023-04-21 | 建研防火科技有限公司 | Intelligent fire control management system based on thing networking |
CN115990327B (en) * | 2023-03-24 | 2023-08-22 | 建研防火科技有限公司 | Intelligent fire control management system based on thing networking |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN112650242A (en) | Mobile robot path planning method based on hybrid algorithm | |
Sun et al. | Probabilistic prediction of interactive driving behavior via hierarchical inverse reinforcement learning | |
CN107168305B (en) | Bezier and VFH-based unmanned vehicle track planning method under intersection scene | |
JP6869562B2 (en) | A method of tracking an object using a CNN including a tracking network, and a device using it {METHOD FOR TRACKING OBJECT BY USING CONVOLUTIONAL NEURAL NETWORK INCLUDING TRACKING NETWORK AND COMPUTING | |
CN110703762B (en) | Hybrid path planning method for unmanned surface vehicle in complex environment | |
CN112882469B (en) | Deep reinforcement learning obstacle avoidance navigation method integrating global training | |
CN111174798A (en) | Foot type robot path planning method | |
CN112034887A (en) | Optimal path training method for unmanned aerial vehicle to avoid cylindrical barrier to reach target point | |
CN110320919B (en) | Method for optimizing path of mobile robot in unknown geographic environment | |
CN112114584A (en) | Global path planning method of spherical amphibious robot | |
CN112577506B (en) | Automatic driving local path planning method and system | |
CN112925315A (en) | Crawler path planning method based on improved ant colony algorithm and A-star algorithm | |
CN112327829A (en) | Distributed multi-robot cooperative motion control method, system, medium and application | |
JP2020123346A (en) | Method and device for performing seamless parameter switching by using location based algorithm selection to achieve optimized autonomous driving in each of regions | |
CN115373399A (en) | Ground robot path planning method based on air-ground cooperation | |
Zhang et al. | A novel informative autonomous exploration strategy with uniform sampling for quadrotors | |
Batinovic et al. | A shadowcasting-based next-best-view planner for autonomous 3D exploration | |
CN112857370A (en) | Robot map-free navigation method based on time sequence information modeling | |
Choi et al. | Improved CNN-based path planning for stairs climbing in autonomous UAV with LiDAR sensor | |
CN116501064A (en) | Path planning and obstacle avoidance method for photovoltaic power station cleaning robot | |
CN116804879A (en) | Robot path planning framework method for improving dung beetle algorithm and fusing DWA algorithm | |
CN113433937B (en) | Hierarchical navigation obstacle avoidance system and hierarchical navigation obstacle avoidance method based on heuristic exploration | |
Cheng et al. | Research on path planning of mobile robot based on dynamic environment | |
CN113867336A (en) | Mobile robot path navigation and planning method suitable for complex scene | |
CN115933637A (en) | Path planning method and device for substation equipment inspection robot 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 | ||
RJ01 | Rejection of invention patent application after publication | ||
RJ01 | Rejection of invention patent application after publication |
Application publication date: 20210413 |