CN116540747A - Mobile robot motion planning and obstacle avoidance decision method with priority of avoidance and stop - Google Patents
Mobile robot motion planning and obstacle avoidance decision method with priority of avoidance and stop Download PDFInfo
- Publication number
- CN116540747A CN116540747A CN202310819907.XA CN202310819907A CN116540747A CN 116540747 A CN116540747 A CN 116540747A CN 202310819907 A CN202310819907 A CN 202310819907A CN 116540747 A CN116540747 A CN 116540747A
- Authority
- CN
- China
- Prior art keywords
- mobile robot
- path
- obstacle
- state
- navigation
- 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.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 48
- 230000008569 process Effects 0.000 claims abstract description 11
- 238000012546 transfer Methods 0.000 claims abstract description 9
- 230000007704 transition Effects 0.000 claims description 39
- 238000004422 calculation algorithm Methods 0.000 claims description 16
- 238000004088 simulation Methods 0.000 claims description 8
- 238000004458 analytical method Methods 0.000 claims description 7
- 230000006870 function Effects 0.000 claims description 6
- 238000012545 processing Methods 0.000 claims description 6
- 238000004364 calculation method Methods 0.000 claims description 4
- 238000005266 casting Methods 0.000 claims description 4
- 230000008859 change Effects 0.000 claims description 4
- 230000000903 blocking effect Effects 0.000 claims description 3
- 230000004888 barrier function Effects 0.000 claims description 2
- 238000013135 deep learning Methods 0.000 abstract description 10
- 238000013461 design Methods 0.000 abstract description 3
- 230000000007 visual effect Effects 0.000 abstract description 3
- 230000004044 response Effects 0.000 abstract description 2
- 238000005070 sampling Methods 0.000 description 12
- 238000005516 engineering process Methods 0.000 description 5
- 238000010586 diagram Methods 0.000 description 3
- 230000006399 behavior Effects 0.000 description 2
- 238000013527 convolutional neural network Methods 0.000 description 2
- 238000011161 development Methods 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 238000013178 mathematical model Methods 0.000 description 2
- 239000000203 mixture Substances 0.000 description 2
- 238000013473 artificial intelligence Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000008094 contradictory effect Effects 0.000 description 1
- 238000013136 deep learning model Methods 0.000 description 1
- 230000001419 dependent effect Effects 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 230000005012 migration Effects 0.000 description 1
- 238000013508 migration Methods 0.000 description 1
- 238000005457 optimization Methods 0.000 description 1
- 230000008447 perception Effects 0.000 description 1
- 238000012549 training Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- 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/0214—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory in accordance with safety or protection criteria, e.g. avoiding hazardous areas
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- 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/0223—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving speed control of the vehicle
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- 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/0257—Control of position or course in two dimensions specially adapted to land vehicles using a radar
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Aviation & Aerospace Engineering (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The invention relates to the technical field of mobile robot navigation, and discloses a mobile robot motion planning and obstacle avoidance decision method with priority of avoidance, which comprises the steps of building a state automaton model, generating alternative path clusters offline, building high-level navigation, building middle-level navigation and building bottom-level navigation. The invention can define the obstacle avoidance rule according to different states and transfer conditions, so that the design and realization of the strategy are more visual and understandable; the method can realize rapid decision and control in a scene with higher real-time requirements, and has high response speed; the method can ensure that a safe and feasible path is preferentially selected in the obstacle avoidance decision process, so that the safety and stability of running are ensured; compared with a navigation obstacle avoidance strategy based on deep learning, the method has lower requirement on data and can reduce dependence on a large amount of data.
Description
Technical Field
The invention relates to the technical field of mobile robot navigation, in particular to a mobile robot motion planning and obstacle avoidance decision method with priority of avoidance and stop.
Background
With the continuous progress of society, the mobile robot navigation technology has been rapidly developed, and the rise in the fields of artificial intelligence, sensors, application electronics and the like has also injected the vivid vitality into the development of the mobile robot navigation technology, so that the mobile robot navigation technology is permeated into various industries and applied to more and more task scenes. Generally, existing navigation obstacle avoidance strategies can be divided into two categories, including a navigation obstacle avoidance strategy based on a traditional control method and a navigation obstacle avoidance strategy based on deep learning.
The traditional control method comprises the following steps:
navigation strategy based on PID control: continuously adjusting parameters such as speed, heading and the like of the mobile robot to enable the mobile robot to be kept on a planned path;
fuzzy control algorithm: and converting the obstacle information acquired by the sensor into fuzzy variables, and then adjusting parameters such as speed and heading of the mobile robot by using a fuzzy control algorithm so as to realize obstacle avoidance and navigation. However, the conventional control method still has some problems.
(1) The traditional control method needs an accurate system mathematical model to effectively model and control the system. However, in practical applications, the complex environment and uncertainty factors faced by the mobile robot are many, and it is difficult to accurately establish a mathematical model, so that the reliability and practicality of the conventional control method are limited.
(2) Poor adaptability to complex environments: the application range of the traditional control method is narrow, the traditional control method is difficult to adapt to complex and changeable environments, and particularly under the condition of encountering unknown obstacles or large uncertainty, the effect of the traditional control method can be greatly influenced.
Navigation obstacle avoidance strategy based on deep learning: with the rapid development of the deep learning technology, the technology is gradually applied to a navigation obstacle avoidance strategy of the mobile robot. For example, convolutional Neural Networks (CNNs) may be used to identify and classify images of the mobile robot surroundings, thereby enabling automatic identification of obstacles and intelligent regulation of obstacle avoidance behavior. But navigation obstacle avoidance strategies based on deep learning also have some problems:
(1) The data volume and computational resource requirements are high: deep learning requires a large amount of data to train and optimize, and requires powerful computational resource support, which is not well suited for some resource-constrained application scenarios.
(2) The model interpretation is poor: deep learning models are usually a black box, and it is difficult to directly understand the decision process and internal structure, which presents certain challenges for reliability and security.
(3) Poor migration: navigation obstacle avoidance strategies based on deep learning are greatly dependent on the characteristics and the scene of training data, so that the navigation obstacle avoidance strategies are difficult to directly migrate to other scenes or tasks.
(4) The real-time requirement is higher: deep learning based models require long inference times, which can create problems for application scenarios with high real-time requirements.
The invention aims to solve the problems of dead zone and detour phenomenon, insufficient optimization and smoothness of the motion path of the mobile robot, high data size requirement of a navigation strategy based on deep learning, poor model interpretation, poor mobility, high real-time requirement and the like existing in the navigation of the traditional control method. By introducing the state automaton model, the navigation obstacle avoidance strategy is easy to understand and explain, strong in flexibility, good in instantaneity, high in reliability and small in data demand.
Disclosure of Invention
In order to solve the technical problems, the invention provides a mobile robot motion planning and obstacle avoidance decision method with priority of avoidance.
In order to solve the technical problems, the invention adopts the following technical scheme:
a mobile robot motion planning and obstacle avoidance decision method with priority of avoidance comprises the following steps:
A. establishing a state automaton model:
the running state and the running state of the mobile robotAbstracting transfer conditions into a state automaton model; the running state of the mobile robot comprises a normal running state, an obstacle avoidance transition state, an obstacle avoidance state and a parking state; normal driving state: when the coverage area of the optimal path is free of obstacles, the mobile robot runs according to the optimal path; obstacle avoidance transition state: reducing the moving speed of the mobile robot, and detecting the stability of the optimal path in real time; obstacle avoidance state: the mobile robot selects a new optimal path to avoid the obstacle; parking status: the mobile robot reduces the speed to zero, waits for a stable optimal path or an operator to give an instruction; wherein when the curvature change between successive points on the optimal path is smaller than the set valueWhen the curvature change between the continuous points on the optimal path is greater than or equal to the set value +.>When the optimal path is in an unstable state;
the transfer conditions included:
a. transition from the normal driving state to the obstacle avoidance transition state: when the mobile robot detects that an obstacle appears on the current optimal path, the mobile robot is switched to an obstacle avoidance transition state; b. transition from the obstacle avoidance transition state to the obstacle avoidance state: when the mobile robot detects that the new optimal path is stable, the mobile robot enters an obstacle avoidance state and switches to the new optimal path to avoid an obstacle; c. transition from obstacle avoidance transition to park: when the mobile robot detects that the optimal path is unstable, the mobile robot is switched to a parking state and waits for obtaining a stable optimal path; or when the mobile robot encounters an unavoidable obstacle, switching to a parking state to wait for an operator instruction; d. transition from the parking state to the normal running state: when an operator gives an instruction for starting a task, the mobile robot is switched to a normal running state; e. transition from the park state to the obstacle avoidance state: when the optimal path is stable; f. transition from obstacle avoidance state to normal travel state: the mobile robot switches back to a normal running state after avoiding the obstacle according to the new optimal path;
B. offline generation of alternative path clusters:
based on the current running state and motion constraint of the mobile robot, forward simulation is performed by using the sampled discrete points to generate a set of different alternative paths which can enable the mobile robot to reach the boundary of the range of the laser radar, and in the navigation process, paths with obstacles blocking the mobile robot are deleted from the set of alternative paths to obtain an alternative path cluster;
when the sampled discrete points are used for forward simulation to generate a set of alternative paths, the method specifically comprises the following steps:
in a complete alternative path cluster, the first samples are divided into 9 groups according to angles, the second, third, fourth and fifth samples are divided into 7, 5, 3 and 1 groups in turn, and the total is generatedAlternate paths; when the angle division is carried out, the left maximum corner and the right maximum corner of the kinematic constraint of the mobile robot are respectively set to be positive and negative, and the mobile robot is evenly divided according to the number of groups sampled each time;
C. building high-level navigation:
on a distance map constructed by a drivable area map and a probability grid map, combining the distance between the nearest obstacle of the mobile robot and the Euclidean distance between the mobile robot and a target point to construct cost values for all grids, planning a global path which is close to the road center and far from the obstacle and provided with global path points by taking the cost values as the planning direction of a heuristic function, and transmitting the global path to a middle layer for navigation;
D. constructing middle layer navigation:
the middle-layer navigation subscribes global path points based on global map issued by the current high-layer navigation and is used for carrying out secondary planning on the global path, including map receiving processing and mixingPlanning;
map receiving processing: switching the sub-map according to the position of the mobile robot in the global map;
mixingAnd a planning module: use of the mix->The planning algorithm performs path planning on the sub map, and issues the planned local path points to the bottom navigation;
E. constructing a bottom layer navigation:
reading an alternative path cluster generated offline and a local path point released by middle-layer navigation, wherein the local path point is a sequence of target points; the mobile robot processes the local point cloud obtained by the laser radar after the terrain analysis according to the positioning information and the height of the point cloud, and the ground point cloud obtained by the terrain analysis part is a drivable area; the point clouds above the ground point cloud H1 are regarded as barriers and classified into non-travelable areas, the non-travelable areas are removed from the corresponding travelable areas, and the point clouds above the ground but below H1 are regarded as slopes; and selecting an optimal path from the alternative path clusters according to an optimal path selection algorithm, directly removing alternative paths exceeding the drivable area, wherein the optimal path has speed and direction information of the mobile robot, and the mobile robot drives according to the obtained speed and direction information.
Further, each alternative path is generated by a cubic spline curve.
Further, constructing the bottom navigation, when selecting an optimal path from the alternative paths according to the optimal path selection algorithm,
(1) Feasible path judgment based on ray casting: on a two-dimensional plane, a ray is at a pointAnd a direction vector->Representation of->Is the included angle between the ray and the x-axis, the distance between the ray and the starting point is tThe coordinates of the points are +.>Wherein->，/>The method comprises the steps of carrying out a first treatment on the surface of the For a given obstacle, the boundary of the obstacle consists of n points, the coordinates of the ith point being +.>，/>The boundary of the obstacle is expressed as n line segments, i line segment is +.>：
；
；
Wherein the method comprises the steps ofRepresenting line segment->The ratio of the upper distance to the starting point; the obstacle is represented by a union of all line segments;
task of intersection point calculation: from the target pointDeparture along direction>Emitting a ray +.>Ray->Line segment i of obstacle +.>The intersection of (2) is expressed as:
；
is the abscissa of the ith point of the obstacle boundary, +.>An ordinate of an i-th point of the obstacle boundary;
ray determinationThe intersection points of all the line segments of the obstacle are selected, and then the intersection point closest to the target point is selected as an effective intersection point;
after changing the direction of the rays according to the alternative path, repeating the task of calculating the intersection points, and judging whether each ray has an effective intersection point with the obstacle or not, if so, the alternative path is not on the feasible path; if not, the alternative path is on a feasible path, so that a feasible path set can be screened out from the alternative path cluster;
(2) Scoring each feasible path in the feasible path set, and selecting an optimal path: defining an included angle between the current position of the mobile robot and the connecting line direction of the target point and the moving direction of the mobile robot as goalDir under the coordinate system of the mobile robot, and an included angle between the current position of the mobile robot and the connecting line direction of the feasible path end point and the moving direction of the mobile robot as MPDIr; scoring of viable pathsThe method comprises the following steps:
；
wherein, the angle difference angdiff= |goldir-mpdir|;
and traversing each feasible path in the feasible path set, wherein the feasible path with the highest score is the optimal path.
Compared with the prior art, the invention has the beneficial technical effects that:
the invention can define the obstacle avoidance rule according to different states and transfer conditions, so that the design and realization of the strategy are more visual and understandable; the state automaton model can be designed and adjusted according to different application scenes, can flexibly add, delete, modify states and transfer conditions, and can adapt to different environments and task requirements; the method can realize rapid decision and control in a scene with higher real-time requirements, and has high response speed; the method can ensure that a safe and feasible path is preferentially selected in the obstacle avoidance decision process, so that the safety and stability of running are ensured; compared with a navigation obstacle avoidance strategy based on deep learning, the method has lower requirement on data and can reduce dependence on a large amount of data.
Drawings
FIG. 1 is a schematic diagram of a navigation system of the present invention;
FIG. 2 is a schematic diagram of an alternative path cluster of the present invention;
FIG. 3 is a flow chart illustrating operation of the navigation system of the present invention;
FIG. 4 is a schematic diagram of a state automaton model of the present invention.
Detailed Description
A preferred embodiment of the present invention will be described in detail with reference to the accompanying drawings.
According to the invention, the obstacle avoidance rules are defined according to different states and transfer conditions, so that the design and implementation of the strategy are more visual and understandable. As shown in fig. 1, the navigation system is composed of three layers of architecture, namely bottom navigation, middle navigation and high navigation, wherein the three layers of navigation respectively complete three large functions on a macroscopic system, namely motion control and obstacle avoidance, local planning according with an AckerMan (AckerMan) model and path planning based on a distance map in a global range.
The invention adopts the following technical scheme:
A. establishing a state automaton model:
and designing a proper state automaton model according to the characteristics of the navigation scene of the mobile robot and the task requirements. In the navigation process of the mobile robot, the running state and the event of the mobile robot are abstracted into a finite state automaton model, wherein the state automaton model comprises an initial state, an intermediate state and an ending state, and transition rules among the states, and the transition rules are used for describing the running state and the behavior decision of the mobile robot under different environments. When the state automaton model is designed, special properties of mobile robot navigation, such as dynamic obstacles, environmental changes and other factors, need to be considered, and meanwhile, the logic of state transition and event triggering conditions need to be considered.
When the state automaton model is established according to the characteristics of a navigation scene of the mobile robot and task requirements, the states comprise a normal running state, an obstacle avoidance transition state, an obstacle avoidance state and a parking state. Normal driving state: when the coverage area of the optimal path selected by the bottom navigation is free of obstacles, the mobile robot runs according to the optimal path. Obstacle avoidance transition state: and reducing the moving speed of the mobile robot, and detecting the stability of the selected optimal path in real time. Obstacle avoidance state: the mobile robot selects a new optimal path to avoid the obstacle. Parking status: the mobile robot drops the speed to zero waiting for a stable optimal path or operator command.
The transfer conditions included: a. transition from the normal driving state to the obstacle avoidance transition state: when the mobile robot detects that an obstacle appears on the current optimal path, the mobile robot needs to be switched to an obstacle avoidance transition state. b. Transition from the obstacle avoidance transition state to the obstacle avoidance state: when the mobile robot detects that the new optimal path is stable, the mobile robot enters an obstacle avoidance state, switches to the new optimal path and avoids the obstacle. c. Transition from obstacle avoidance transition to park: when the mobile robot detects that the fluctuation of the optimal path is large and unstable, the mobile robot is switched to a parking state, and the mobile robot waits for obtaining a stable optimal path or encounters an unavoidable obstacle, the mobile robot needs to be switched to the parking state to wait for an operator instruction. d. Transition from the parking state to the normal running state: when the operator gives an instruction to start a task, the mobile robot switches back to the normal running state. e. Transition from the park state to the obstacle avoidance state: and when the optimal path is stable. f. From obstacle avoidance state to normal driving state: and after the mobile robot avoids the obstacle according to the new optimal path, switching back to the normal running state. The above is the state and transition condition of the state automaton model in this embodiment, and is specifically shown in fig. 4.
B. Offline generation of alternative path clusters:
the offline alternative path cluster is a set of different alternative paths which are generated by forward simulation by using sampled discrete points and can reach the boundary of the range of the sensing sensor (laser radar) based on the current state of the mobile robot, and the possible situation in a range can be predicted. These alternative paths are generated based on vehicle motion constraints, each generated by a cubic spline, and a set of alternative path clusters can be considered as viable paths from the starting state to the boundary of the sensing sensor range. During navigation, a perception sensor (laser radar) detects obstacles blocking some paths, the part of paths are deleted, and only a subset of the trafficable paths is reserved, so that a feasible path set is obtained. The alternative path clusters are generated offline, in a complete alternative path cluster, the first sampling group groupID is set to 9 groups, and in order to reduce the consistency of the sensor boundary paths, the alternative paths after the second, third, fourth and fifth sampling are sequentially set to 7, 5, 3 and 1 groups, and the total generation is performedAlternate paths are striped and the corresponding pathids are named. And respectively setting the left maximum rotation angle and the right maximum rotation angle to be positive and negative according to the maximum rotation angle of the kinematic constraint of the vehicle, and uniformly dividing the left maximum rotation angle and the right maximum rotation angle into 9, 7, 5 and 3 sections according to sampling times to be used as the angle constraint of each section of alternative path. The length constraint of each segment of the alternative path is set to 1 meter.
The specific method for generating the simulation alternative path by using matlab is as follows: defining movementThe maximum turning angle of the robot is angle, the first sampled angle offset step deltaAngle 1=angle/4, so that it can be generatedA path. Taking the first path as an example, the offset shift1 is-angle, the radius dis of each path is 1, and the array wayptssstart= [0, 0; dis, shift1,0]For storing the start and end positions of the first sampling alternative path. The computer code employed is specifically as follows:
pathStartR=0:0.01:dis；
pathStartShift=spline(wayptsStart(:,1),wayptsStart(:,2),pathStartR)；
100 times of data are uniformly interpolated in the interval 0 to 1, and 101 angles are put into a row vector pathStartShift to be stored. The computer code employed is specifically as follows:
pathStartX=pathStartR .cos(pathStartShift/>pi /180)；
pathStartY=pathStartR .sin(pathStartShift/>pi /180)；
pathStartZ=zeros(size(pathStartX))。
the path startshift is used for changing the straight line of the path startr from the 0 interval to the 1 interval into a curve, and an alternative path after the first sampling is obtained.
Because the adopted quintic spline simulation path is calculated by shift 1= -angle: deltaAngle1: angle, setting an angle offset step length deltaAngle 2=angle/3 of the second sampling, an angle offset step length deltaAngle 3=angle/2 of the third sampling and an angle offset step length deltaAngle 4=angle of the fourth sampling in the follow-up simulation spline, so that the number of the sampled alternative paths is 9, 7, 5, 3 and 1 in sequence. The principle of the subsequent sampling is the same as the first sampling. The computer code employed is specifically as follows:
for shift2 = -anglescale+shift1:deltaAngle2/>scale:angle/>scale+shift1
for shift3 = -anglescale^2+shift2:deltaAngle3/>scale^2:angle/>scale^2+shift2
for shift4=-anglescale^3+shift3:deltaAngle4/>scale^3:angle/>scale^3+shift3
for shift5=shift4
waypts=[pathStartR’, pathStartShift' . pathStartZ’；
2dis, shift2, 0；
3dis, shift3, 0；
4dis, shift4, 0；
5dis - 0.001. shift5, 0；
5dis, shift5, 0]；
pathR=0 : 0.01 : waypts(end, 1)；
pathShift = spline(waypts(:, 1), waypts(:, 2), pathR)；
pathX=pathR .cos (pathShift/>pi/180)；
pathY=pathR .sin (pathShift/>pi/180)；
pathZ=zeros (size (pathX))；
plot3 (pathX, pathY, path2)；
end
end
end
end。
compared with the first sampling, the subsequent sampling angles are scaled by a certain proportion, and the pathR at the moment is in a range from 0-1 to 0-5 compared with the pathStartR at the first timeBecomes the row vector of (2)It is the same that the value of pathR is still incremented by 0.01. An alternative path cluster as in fig. 2 is finally obtained.
The alternative path clusters are generated based on vehicle motion constraint, and each alternative path generates a cubic spline curve, so that the obstacle avoidance path of the mobile robot is more stable and smooth.
C. Building high-level navigation:
the high-level navigation is used for planning a path on the global grid map, sending global path points obtained by path planning to the middle-level navigation, and sending the deconstructed middle-level navigation to the bottom-level navigation. The map information source of the high-level navigation is a grid map formed by rasterizing a point cloud map spliced by using RTK pose. If the grid map used in the special scene cannot meet the requirement, a certain number of points are required to be extracted on the basis of the point cloud map, and a unit circle coverage algorithm is used for generating the topological map. And on a distance map constructed by the drivable area map and the probability grid map, constructing cost values for all grids through the distance between the nearest obstacle and the Euclidean distance between the nearest obstacle and the target point, and planning a high-quality global path close to the road center and far away from the obstacle by taking the cost values as the planning direction of a heuristic function. The planned global Path is sent to the middle navigation layer in the form of nav_msgs:: path message which contains a series of global Path points.
D. Constructing middle layer navigation:
the role played by middle navigation in the whole navigation system is mainly responsible for carrying out secondary planning on a global path planned by high-level navigation, and mainly comprises two modules: map receiving and processing module and mixingAnd a planning module.
Map receiving and processing module: the sub-map (local map) is switched according to the position of the mobile robot in the current global map.
MixingAnd a planning module: use of mix +.on sub-map>Path planning by algorithmAnd issuing the planned local path points to the underlying navigation.
The middle-layer navigation subscribes to global path points based on global map issued by the high-layer navigation, in general, the global path points are planned in a global scope, so that the distance between the two global path points is far, if the bottom-layer navigation is only used for receiving the global path points from the high-layer navigation, effective traction cannot be formed in a long distance between the two global path points, and the bottom-layer navigation is trapped in a local optimal place and possibly blocked in a certain position. This is also one of the reasons for adding middle navigation. And the middle navigation involves sub-map switching in the global scope, and the sub-map is automatically switched according to the position of the current global path point and the inclusion relation of the sub-map.
In order to solve the problem that the grid size and the vehicle body size are contradictory between the Ackerman vehicle body and the grid map, the middle layer navigation uses a mixtureThe algorithm processes the map. Because the bottom layer navigation is unknown to the outside information except for the point cloud in the local range, the introduction of the middle layer navigation is indispensable.
Using distance-based mapsAlgorithm solving the existing ++on global travelable area map>The algorithm takes Euclidean distance as a cost value of heuristic function reference to plan a path closer to an obstacle, and when the condition that obstacle avoidance is needed is faced, the free space for the mobile robot to avoid the obstacle is smaller, so that the problem of smoothness and safety of running of the mobile robot are affected.
E. Constructing a bottom layer navigation:
the bottom navigation is a key module for the whole navigation system to interact with the chassis, and is a converter for the navigation system to practically apply the algorithm to the chassis. Firstly, reading an offline generated alternative path cluster, storing the motion meta-information in a corresponding txt file in the form of points for reducing the reading cost, reading before the bottom navigation is started, and storing in a memory in a container mode after reading.
The mobile robot can be positioned in real time according to RTK or positioned information obtained by a positioning algorithm in a global priori map in the running process, and the mobile robot is processed according to the height of the point cloud according to the local point cloud provided by the Velodyne laser radar and subjected to terrain analysis: being regarded as an obstacle 15cm higher than the ground point cloud, the mobile robot cannot pass through, is classified into a non-travelable orientation and rejects the part in a corresponding travelable area. While points above the ground but below 15cm would be considered ramp portions. And carrying out omnibearing balance according to the shielding information, the topographic analysis information and the target point azimuth information of the obstacle, selecting an optimal path from the alternative paths according to an optimal path selection algorithm, wherein the optimal path carries corresponding speed (angular speed and linear speed) and direction information, and driving a vehicle body after obtaining the corresponding information. In the process of selecting the optimal path, the direction analysis of the target point is quite clear, namely, the bottom navigation can preferentially select the path closest to the current target point. The target point refers to the next target position to which the mobile robot is going at the current position, and the selection principle and method of the optimal path are as follows:
(1) Feasible path judgment based on ray casting (Raycasting): raycasting is an algorithm based on geometric calculation, which is used for detecting problems such as collision, visibility and the like in a scene. The basic idea of ray casting is to determine a ray from a point, determine if the ray intersects an object in the scene, and thus obtain a collision point or visibility. On a two-dimensional plane, a ray may be used as a pointAnd a direction vector->Representation of->Is the angle between the ray and the x-axis, the coordinate of the point on the ray with the distance t from the starting point is +.>Wherein->，The method comprises the steps of carrying out a first treatment on the surface of the For a given obstacle, it can be represented as a polygon or a set of line segments, the boundary of the obstacle is composed of n points, each point having the coordinates +.>，/>The boundary of the obstacle is expressed as n line segments, i line segment is +.>：
；
；
Wherein the method comprises the steps ofRepresenting line segment->The ratio of the upper distance to the starting point; the obstacle is represented by a union of all line segments.
Task of intersection point calculation: from the target pointDeparture along direction>Emitting a ray +.>Detecting whether the ray intersects with the obstacle, ray +.>The parametric equation for (2) is:
。
then rayLine segment i of obstacle +.>The intersection of (2) is expressed as:
；
calculate the outgoing lineThe intersection points of all the line segments of the obstacle are selected, and then the intersection point closest to the target point is selected as an effective intersection point;
after changing the direction of the rays according to the alternative path, repeating the task of calculating the intersection points, and judging whether each ray has an effective intersection point with the obstacle or not, if so, the alternative path is not on the feasible path; if not, the alternative path is on a feasible path, so that a feasible path set can be screened out from the alternative path cluster.
(2) Scoring each feasible path of the feasible path set, and selecting an optimal path: defining under a coordinate system of the mobile robot, wherein an included angle between a connecting line direction of the current position of the mobile robot and the target point and a moving direction of the mobile robot is goalDir, and an included angle between a connecting line direction of the current position of the mobile robot and the end point of a feasible path and the moving direction of the mobile robot is MPDIR; scoring of viable pathsThe method comprises the following steps:
；
the smaller the angle difference angdiff= |goaldir-mpdir| is, which means that the closer the end point of the feasible path is to the target point, the higher the score of the feasible path should be. A square root function is used here to normalize the angle difference, ensuring that the feasible path score is in the range of 0, 1. Traversing each feasible path in the feasible path set, wherein the feasible path with the highest score is the optimal path.
The flow of operation of the navigation system of the present invention is shown in fig. 3.
It will be evident to those skilled in the art that the invention is not limited to the details of the foregoing illustrative embodiments, and that the present invention may be embodied in other specific forms without departing from the spirit or essential characteristics thereof. The present embodiments are therefore to be considered in all respects as illustrative and not restrictive, the scope of the invention being indicated by the appended claims rather than by the foregoing description, and all changes which come within the meaning and range of equivalency of the claims are therefore intended to be embraced therein.
Furthermore, it should be understood that although the present disclosure describes embodiments, not every embodiment is provided with a single embodiment, and that this description is provided for clarity only, and that the disclosure is not limited to specific embodiments, and that the embodiments may be combined appropriately to form other embodiments that will be understood by those skilled in the art.
Claims (3)
1. A mobile robot motion planning and obstacle avoidance decision method with priority of avoidance comprises the following steps:
A. establishing a state automaton model:
abstracting the running state and the transfer condition of the mobile robot into a state automaton model; the running state of the mobile robot comprises a normal running state, an obstacle avoidance transition state, an obstacle avoidance state and a parking state; normal driving state: when the coverage area of the optimal path is free of obstacles, the mobile robot runs according to the optimal path; obstacle avoidance transition state: reducing the moving speed of the mobile robot, and detecting the stability of the optimal path in real time; obstacle avoidance state: the mobile robot selects a new optimal path to avoid the obstacle; parking status: the mobile robot reduces the speed to zero, waits for a stable optimal path or an operator to give an instruction; wherein when the curvature change between successive points on the optimal path is smaller than the set valueWhen the curvature change between the continuous points on the optimal path is greater than or equal to the set value +.>When the optimal path is in an unstable state;
the transfer conditions included:
a. transition from the normal driving state to the obstacle avoidance transition state: when the mobile robot detects that an obstacle appears on the current optimal path, the mobile robot is switched to an obstacle avoidance transition state; b. transition from the obstacle avoidance transition state to the obstacle avoidance state: when the mobile robot detects that the new optimal path is stable, the mobile robot enters an obstacle avoidance state and switches to the new optimal path to avoid an obstacle; c. transition from obstacle avoidance transition to park: when the mobile robot detects that the optimal path is unstable, the mobile robot is switched to a parking state and waits for obtaining a stable optimal path; or when the mobile robot encounters an unavoidable obstacle, switching to a parking state to wait for an operator instruction; d. transition from the parking state to the normal running state: when an operator gives an instruction for starting a task, the mobile robot is switched to a normal running state; e. transition from the park state to the obstacle avoidance state: when the optimal path is stable; f. transition from obstacle avoidance state to normal travel state: the mobile robot switches back to a normal running state after avoiding the obstacle according to the new optimal path;
B. offline generation of alternative path clusters:
based on the current running state and motion constraint of the mobile robot, forward simulation is performed by using the sampled discrete points to generate a set of different alternative paths which can enable the mobile robot to reach the boundary of the range of the laser radar, and in the navigation process, paths with obstacles blocking the mobile robot are deleted from the set of alternative paths to obtain an alternative path cluster;
when the sampled discrete points are used for forward simulation to generate a set of alternative paths, the method specifically comprises the following steps:
in a complete alternative path cluster, the first samples are divided into 9 groups according to angles, the second, third, fourth and fifth samples are divided into 7, 5, 3 and 1 groups in turn, and the total is generatedAlternate paths; when the angle division is carried out, the left maximum corner and the right maximum corner of the kinematic constraint of the mobile robot are respectively set to be positive and negative, and the mobile robot is evenly divided according to the number of groups sampled each time;
C. building high-level navigation:
on a distance map constructed by a drivable area map and a probability grid map, combining the distance between the nearest obstacle of the mobile robot and the Euclidean distance between the mobile robot and a target point to construct cost values for all grids, planning a global path which is close to the road center and far from the obstacle and provided with global path points by taking the cost values as the planning direction of a heuristic function, and transmitting the global path to a middle layer for navigation;
D. constructing middle layer navigation:
the middle-layer navigation subscribes global path points based on global map issued by the current high-layer navigation and is used for carrying out secondary planning on the global path, including map receiving processing and mixingPlanning;
map receiving processing: switching the sub-map according to the position of the mobile robot in the global map;
mixingAnd a planning module: use of the mix->The planning algorithm performs path planning on the sub map, and issues the planned local path points to the bottom navigation;
E. constructing a bottom layer navigation:
reading an alternative path cluster generated offline and a local path point released by middle-layer navigation, wherein the local path point is a sequence of target points; the mobile robot processes the local point cloud obtained by the laser radar after the terrain analysis according to the positioning information and the height of the point cloud, and the ground point cloud obtained by the terrain analysis part is a drivable area; the point clouds above the ground point cloud H1 are regarded as barriers and classified into non-travelable areas, the non-travelable areas are removed from the corresponding travelable areas, and the point clouds above the ground but below H1 are regarded as slopes; and selecting an optimal path from the alternative path clusters according to an optimal path selection algorithm, directly removing alternative paths exceeding the drivable area, wherein the optimal path has speed and direction information of the mobile robot, and the mobile robot drives according to the obtained speed and direction information.
2. The avoidance priority mobile robot motion planning and obstacle avoidance decision method of claim 1, wherein: each alternative path is generated from a cubic spline curve.
3. The avoidance priority mobile robot motion planning and obstacle avoidance decision method of claim 1, wherein: constructing a bottom navigation, when selecting an optimal path from the alternative paths according to an optimal path selection algorithm,
(1) Feasible path judgment based on ray casting:on a two-dimensional plane, a ray is at a pointAnd direction vectorRepresentation of->Is the angle between the ray and the x-axis, the coordinate of the point on the ray with the distance t from the starting point is +.>Wherein->，/>The method comprises the steps of carrying out a first treatment on the surface of the For a given obstacle, the boundary of the obstacle consists of n points, the coordinates of the ith point being +.>，/>The boundary of the obstacle is expressed as n line segments, i line segment is +.>：
；
；
Wherein the method comprises the steps ofRepresenting line segment->The ratio of the upper distance to the starting point; the obstacle is represented by a union of all line segments;
task of intersection point calculation: from the target pointDeparture along direction>Emitting a ray +.>Ray->Line segment i of obstacle +.>The intersection of (2) is expressed as:
；
is the abscissa of the ith point of the obstacle boundary, +.>An ordinate of an i-th point of the obstacle boundary;
ray determinationThe intersection points of all the line segments of the obstacle are selected, and then the intersection point closest to the target point is selected as an effective intersection point;
after changing the direction of the rays according to the alternative path, repeating the task of calculating the intersection points, and judging whether each ray has an effective intersection point with the obstacle or not, if so, the alternative path is not on the feasible path; if not, the alternative path is on a feasible path, so that a feasible path set can be screened out from the alternative path cluster;
(2) Scoring each feasible path in the feasible path set, and selecting an optimal path: defining an included angle between the current position of the mobile robot and the connecting line direction of the target point and the moving direction of the mobile robot as goalDir under the coordinate system of the mobile robot, and an included angle between the current position of the mobile robot and the connecting line direction of the feasible path end point and the moving direction of the mobile robot as MPDIr; scoring of viable pathsThe method comprises the following steps:
；
wherein, the angle difference angdiff= |goldir-mpdir|;
and traversing each feasible path in the feasible path set, wherein the feasible path with the highest score is the optimal path.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310819907.XA CN116540747B (en) | 2023-07-06 | 2023-07-06 | Mobile robot motion planning and obstacle avoidance decision method with priority of avoidance and stop |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310819907.XA CN116540747B (en) | 2023-07-06 | 2023-07-06 | Mobile robot motion planning and obstacle avoidance decision method with priority of avoidance and stop |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN116540747A true CN116540747A (en) | 2023-08-04 |
| CN116540747B CN116540747B (en) | 2023-10-20 |
Family
ID=87443946
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310819907.XA Active CN116540747B (en) | 2023-07-06 | 2023-07-06 | Mobile robot motion planning and obstacle avoidance decision method with priority of avoidance and stop |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116540747B (en) |
Citations (12)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105303595A (en) * | 2014-07-30 | 2016-02-03 | Tcl集团股份有限公司 | Three-dimensional virtual scene-based intelligent obstacle avoiding method and system |
| CN108241375A (en) * | 2018-02-05 | 2018-07-03 | 景德镇陶瓷大学 | A kind of application process of self-adaptive genetic operator in mobile robot path planning |
| CN110045737A (en) * | 2019-04-28 | 2019-07-23 | 南京邮电大学 | The path planning of apery Soccer robot based on dynamic window method |
| US20200158862A1 (en) * | 2018-11-19 | 2020-05-21 | Invensense, Inc. | Method and system for positioning using radar and motion sensors |
| JP2020135560A (en) * | 2019-02-21 | 2020-08-31 | 新東工業株式会社 | Autonomous mobile robot |
| CN111949032A (en) * | 2020-08-18 | 2020-11-17 | 中国科学技术大学 | 3D obstacle avoidance navigation system and method based on reinforcement learning |
| US20210118230A1 (en) * | 2019-10-18 | 2021-04-22 | Sony Interactive Entertainment Inc. | Navigation system and method |
| CN113847920A (en) * | 2020-06-25 | 2021-12-28 | 图森有限公司 | Two-stage path planning for autonomous vehicles |
| WO2022083292A1 (en) * | 2020-10-23 | 2022-04-28 | 苏州大学 | Smart mobile robot global path planning method and system |
| CN115123310A (en) * | 2022-08-31 | 2022-09-30 | 中汽数据(天津)有限公司 | Unmanned vehicle obstacle avoidance local path planning method, device and storage medium |
| CN115946111A (en) * | 2022-12-09 | 2023-04-11 | 长春理工大学 | Robot obstacle avoidance method based on deep learning panoramic camera |
| CN116147654A (en) * | 2023-04-19 | 2023-05-23 | 广东工业大学 | Path planning method based on offline path library |
-
2023
- 2023-07-06 CN CN202310819907.XA patent/CN116540747B/en active Active
Patent Citations (12)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105303595A (en) * | 2014-07-30 | 2016-02-03 | Tcl集团股份有限公司 | Three-dimensional virtual scene-based intelligent obstacle avoiding method and system |
| CN108241375A (en) * | 2018-02-05 | 2018-07-03 | 景德镇陶瓷大学 | A kind of application process of self-adaptive genetic operator in mobile robot path planning |
| US20200158862A1 (en) * | 2018-11-19 | 2020-05-21 | Invensense, Inc. | Method and system for positioning using radar and motion sensors |
| JP2020135560A (en) * | 2019-02-21 | 2020-08-31 | 新東工業株式会社 | Autonomous mobile robot |
| CN110045737A (en) * | 2019-04-28 | 2019-07-23 | 南京邮电大学 | The path planning of apery Soccer robot based on dynamic window method |
| US20210118230A1 (en) * | 2019-10-18 | 2021-04-22 | Sony Interactive Entertainment Inc. | Navigation system and method |
| CN113847920A (en) * | 2020-06-25 | 2021-12-28 | 图森有限公司 | Two-stage path planning for autonomous vehicles |
| CN111949032A (en) * | 2020-08-18 | 2020-11-17 | 中国科学技术大学 | 3D obstacle avoidance navigation system and method based on reinforcement learning |
| WO2022083292A1 (en) * | 2020-10-23 | 2022-04-28 | 苏州大学 | Smart mobile robot global path planning method and system |
| CN115123310A (en) * | 2022-08-31 | 2022-09-30 | 中汽数据(天津)有限公司 | Unmanned vehicle obstacle avoidance local path planning method, device and storage medium |
| CN115946111A (en) * | 2022-12-09 | 2023-04-11 | 长春理工大学 | Robot obstacle avoidance method based on deep learning panoramic camera |
| CN116147654A (en) * | 2023-04-19 | 2023-05-23 | 广东工业大学 | Path planning method based on offline path library |
Non-Patent Citations (3)
| Title |
|---|
| 欧为祥;陆泽青;朱达群;陈光磊;: "基于激光雷达的移动机器人室内定位与导航", 电子世界, no. 23, pages 146 - 147 * |
| 赵毅;卓永宁;: "一种室内射线跟踪的二维模型实现", 微计算机信息, no. 34, pages 15 - 22 * |
| 陈宗海，陈锋: "一种不确定环境下移动机器人的避障规划算法", 《机器人》, vol. 24, no. 4, pages 358 - 361 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN116540747B (en) | 2023-10-20 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Sharma et al. | Recent advances in motion and behavior planning techniques for software architecture of autonomous vehicles: A state-of-the-art survey | |
| CN111338346B (en) | Automatic driving control method and device, vehicle and storage medium | |
| Chen et al. | Conditional DQN-based motion planning with fuzzy logic for autonomous driving | |
| Stahl et al. | Multilayer graph-based trajectory planning for race vehicles in dynamic scenarios | |
| Li et al. | Real-time trajectory planning for autonomous urban driving: Framework, algorithms, and verifications | |
| Zhao et al. | Dynamic motion planning for autonomous vehicle in unknown environments | |
| CN110992695B (en) | Vehicle urban intersection traffic decision multi-objective optimization method based on conflict resolution | |
| CN112577506B (en) | Automatic driving local path planning method and system | |
| Li et al. | A mobile robot path planning algorithm based on improved A* algorithm and dynamic window approach | |
| Li et al. | A practical trajectory planning framework for autonomous ground vehicles driving in urban environments | |
| CN116804879A (en) | Robot path planning framework method for improving dung beetle algorithm and fusing DWA algorithm | |
| Chen et al. | Target-driven obstacle avoidance algorithm based on DDPG for connected autonomous vehicles | |
| Yang et al. | Driving space for autonomous vehicles | |
| Gao et al. | Cloud model approach for lateral control of intelligent vehicle systems | |
| CN116331264A (en) | Obstacle avoidance path robust planning method and system for unknown obstacle distribution | |
| Gao et al. | Autonomous driving of vehicles based on artificial intelligence | |
| Sun et al. | Interactive left-turning of autonomous vehicles at uncontrolled intersections | |
| Yu et al. | Hierarchical framework integrating rapidly-exploring random tree with deep reinforcement learning for autonomous vehicle | |
| Reda et al. | Path planning algorithms in the autonomous driving system: A comprehensive review | |
| CN115230729A (en) | Automatic driving obstacle avoidance method and system and storage medium | |
| CN116540747B (en) | Mobile robot motion planning and obstacle avoidance decision method with priority of avoidance and stop | |
| CN116448134B (en) | Vehicle path planning method and device based on risk field and uncertain analysis | |
| Smit et al. | Informed sampling-based trajectory planner for automated driving in dynamic urban environments | |
| Rosero et al. | CNN-Planner: A neural path planner based on sensor fusion in the bird's eye view representation space for mapless autonomous driving | |
| Wu et al. | Smooth path planning method of agricultural vehicles based on improved Hybrid A |
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 |