CN115016510A - Robot navigation obstacle avoidance method and device and storage medium - Google Patents
Robot navigation obstacle avoidance method and device and storage medium Download PDFInfo
- Publication number
- CN115016510A CN115016510A CN202210944226.1A CN202210944226A CN115016510A CN 115016510 A CN115016510 A CN 115016510A CN 202210944226 A CN202210944226 A CN 202210944226A CN 115016510 A CN115016510 A CN 115016510A
- Authority
- CN
- China
- Prior art keywords
- target navigation
- original
- robot
- initial position
- navigation position
- 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 52
- 238000004458 analytical method Methods 0.000 claims abstract description 16
- 238000013433 optimization analysis Methods 0.000 claims abstract description 12
- 230000006870 function Effects 0.000 claims description 119
- 238000011156 evaluation Methods 0.000 claims description 58
- 230000008569 process Effects 0.000 claims description 20
- 239000000126 substance Substances 0.000 claims description 16
- 238000004590 computer program Methods 0.000 claims description 11
- 238000005457 optimization Methods 0.000 claims description 10
- 238000005070 sampling Methods 0.000 claims description 5
- 238000012216 screening Methods 0.000 claims description 5
- 230000004888 barrier function Effects 0.000 claims description 2
- 238000004422 calculation algorithm Methods 0.000 description 17
- 230000001133 acceleration Effects 0.000 description 4
- 230000009471 action Effects 0.000 description 3
- 238000010586 diagram Methods 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 230000003247 decreasing effect Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 238000007689 inspection Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 238000012795 verification 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, 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/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
-
- 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
-
- 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/0268—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means
- G05D1/027—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means comprising intertial navigation means, e.g. azimuth detector
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- Aviation & Aerospace Engineering (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Electromagnetism (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The invention provides a robot navigation obstacle avoidance method, a device and a storage medium, belonging to the field of robot navigation, wherein the method comprises the following steps: constructing a grid map through a plurality of original distances; carrying out global path analysis on the initial position, the original moving angle, the target navigation position, the grid map and the target navigation position to obtain a global path and a horizontal coordinate difference value between the initial position and the target navigation position; and according to the original movement angle, performing path optimization analysis on the current movement angle, the horizontal coordinate difference value between the initial position and the target navigation position, the global path, the target navigation position, the grid map and a plurality of current distances to obtain an optimized path until the target navigation position is reached. The invention optimizes the path planning of the robot, ensures the globally optimal route, and simultaneously realizes the capability of the mobile robot to avoid dynamic obstacles and reach a target point in a complex environment.
Description
Technical Field
The invention mainly relates to the technical field of robot navigation, in particular to a robot navigation obstacle avoidance method, a device and a storage medium.
Background
With the rapid development of robotics, more and more robots are applied to various complicated environments. The robot autonomous navigation obstacle avoidance is one of basic problems of robot technology, and the robot usually establishes a two-dimensional grid map, and gray values in the grid indicate whether the point position is a free space or an obstacle, so as to perform a path planning algorithm. In most application scenarios, the environment of the robot is partially known and partially unknown, and for this case, a global planning path from the starting point to the target point should be planned according to the global environment information. In the process that a robot travels along a global planned path, how to select a proper local obstacle avoidance method to avoid an obstacle when the robot encounters an unknown obstacle is a problem to be solved at present.
Disclosure of Invention
The invention provides a method and a device for robot navigation and obstacle avoidance and a storage medium, aiming at the defects of the prior art.
The technical scheme for solving the technical problems is as follows: a robot navigation obstacle avoidance method comprises the following steps:
s1: acquiring distances from emitted laser to a plurality of objects in a preset area through a two-dimensional laser radar arranged on a robot to obtain a plurality of original distances, and constructing a grid map through the original distances;
s2: a target navigation position is led in, an initial position is obtained through the two-dimensional laser radar, an original moving angle is obtained through a gyroscope sensor arranged on the robot, global path analysis is carried out on the initial position, the original moving angle, the target navigation position, the grid map and the target navigation position, a global path and a horizontal coordinate difference value between the initial position and the target navigation position are obtained, and the robot is controlled to move in the grid map along the global path;
s3: acquiring distances from the laser emitted by the current position of the robot to a plurality of objects through the two-dimensional laser radar to obtain a plurality of current distances, and obtaining a current moving angle through the gyroscope sensor;
s4: and performing path optimization analysis on the current movement angle, the horizontal coordinate difference value between the initial position and the target navigation position, the global path, the target navigation position, the grid map and the current distances according to the original movement angle to obtain an optimized path, controlling the robot to move in the grid map along the optimized path, and returning to the step S3 until the target navigation position is reached.
Another technical solution of the present invention for solving the above technical problems is as follows: a robot navigation obstacle avoidance device, comprising:
the map building module is used for acquiring distances from emitted laser to a plurality of objects in a preset area through a two-dimensional laser radar arranged on the robot to obtain a plurality of original distances and building a grid map through the original distances;
the global path analysis module is used for leading in a target navigation position, obtaining an initial position through the two-dimensional laser radar, obtaining an original movement angle through a gyroscope sensor arranged on the robot, performing global path analysis on the initial position, the original movement angle, the target navigation position, the grid map and the target navigation position to obtain a global path and a horizontal coordinate difference value between the initial position and the target navigation position, and controlling the robot to move in the grid map along the global path;
the data acquisition module is used for acquiring the distances from the laser emitted by the current position of the robot to a plurality of objects through the two-dimensional laser radar to obtain a plurality of current distances and obtaining a current moving angle through the gyroscope sensor;
and the path optimization analysis module is used for performing path optimization analysis on the current movement angle, the horizontal coordinate difference value between the initial position and the target navigation position, the global path, the target navigation position, the grid map and the plurality of current distances according to the original movement angle to obtain an optimized path, controlling the robot to move in the grid map along the optimized path, and returning to the data acquisition module until the target navigation position is reached.
Another technical solution of the present invention for solving the above technical problems is as follows: a robot navigation obstacle avoidance device comprises a memory, a processor and a computer program which is stored in the memory and can run on the processor, and when the processor executes the computer program, the robot navigation obstacle avoidance method is realized.
Another technical solution of the present invention for solving the above technical problems is as follows: a computer-readable storage medium, storing a computer program which, when executed by a processor, implements a robot navigation obstacle avoidance method as described above.
The invention has the beneficial effects that: constructing a grid map by a plurality of the original distances, analyzing the initial position, the original movement angle, the target navigation position, the grid map and the global path of the target navigation position to obtain a global path and a horizontal coordinate difference value of the initial position and the target navigation position, controlling the robot to move in the grid map along the global path, obtaining an optimized path by optimizing and analyzing the current movement angle, the horizontal coordinate difference value between the initial position and the target navigation position, the global path, the target navigation position, the grid map and a plurality of paths of current distances according to the original movement angle, and controls the robot to move in the grid map along the optimized path until reaching the target navigation position, optimizes the path planning of the robot, ensures the globally optimal path, meanwhile, the capability that the mobile robot can avoid dynamic obstacles and reach a target point in a complex environment is realized.
Drawings
Fig. 1 is a schematic flow chart of a robot navigation obstacle avoidance method according to an embodiment of the present invention;
fig. 2 is a block diagram of a robot navigation obstacle avoidance device according to an embodiment of the present invention.
Detailed Description
The principles and features of this invention are described below in conjunction with the following drawings, which are set forth by way of illustration only and are not intended to limit the scope of the invention.
Fig. 1 is a schematic flow chart of a robot navigation obstacle avoidance method according to an embodiment of the present invention.
As shown in fig. 1, a robot navigation obstacle avoidance method includes the following steps:
s1: acquiring distances from emitted laser to a plurality of objects in a preset area through a two-dimensional laser radar arranged on a robot to obtain a plurality of original distances, and constructing a grid map through the original distances;
s2: a target navigation position is led in, an initial position is obtained through the two-dimensional laser radar, an original moving angle is obtained through a gyroscope sensor arranged on the robot, global path analysis is carried out on the initial position, the original moving angle, the target navigation position, the grid map and the target navigation position, a global path and a horizontal coordinate difference value between the initial position and the target navigation position are obtained, and the robot is controlled to move in the grid map along the global path;
s3: acquiring distances from the laser emitted by the current position of the robot to a plurality of objects through the two-dimensional laser radar to obtain a plurality of current distances, and obtaining a current moving angle through the gyroscope sensor;
s4: and performing path optimization analysis on the current movement angle, the horizontal coordinate difference value between the initial position and the target navigation position, the global path, the target navigation position, the grid map and the current distances according to the original movement angle to obtain an optimized path, controlling the robot to move in the grid map along the optimized path, and returning to the step S3 until the target navigation position is reached.
Preferably, the type of the gyro sensor may be an MPU6050 gyro sensor.
It should be understood that the environment in which the robot is located is simulated and mapped, represented as the grid map.
Specifically, a target navigation point (i.e., the target navigation position) is set on the grid map, and a global navigation path (i.e., the global path) is planned according to the position of an obstacle appearing on the path by using an a-x algorithm added with a new heuristic function.
Specifically, on a planned global navigation path (i.e., the global path), a DWA algorithm added with a new distance evaluation function is used to optimize a local obstacle avoidance navigation path so that the local obstacle avoidance navigation path fits the planned global navigation path (i.e., the global path).
It should be understood that the robot is controlled to move by using the upper computer software, and the raw data (i.e. the initial position and the plurality of the current distances) of the sensors are acquired by the two-dimensional lidar carried by the mobile robot.
In the above embodiment, a grid map is constructed by a plurality of the original distances, a global path and a difference between horizontal coordinates of the original position and the target navigation position are obtained by analyzing the initial position, the original movement angle, the target navigation position, the grid map and the global path of the target navigation position, and the robot is controlled to move in the grid map along the global path, according to the original moving angle, the difference value of the horizontal coordinates of the initial position and the target navigation position, the global path, the target navigation position, the grid map and a plurality of paths of the current distance, the optimized path is obtained by optimizing and analyzing, and controls the robot to move in the grid map along the optimized path until reaching the target navigation position, optimizes the path planning of the robot, ensures the globally optimal path, meanwhile, the capability that the mobile robot can avoid dynamic obstacles and reach a target point in a complex environment is realized.
Optionally, as an embodiment of the present invention, in step S2, performing a global path analysis on the initial position, the original movement angle, the target navigation position, the grid map, and the target navigation position, and obtaining a global path and a difference between horizontal coordinates of the initial position and the target navigation position includes:
calculating a heuristic function on the initial position, the original movement angle and the target navigation position to obtain the heuristic function and a horizontal coordinate difference value between the initial position and the target navigation position;
calculating an evaluation function through a first formula to the heuristic function to obtain the evaluation function, wherein the first formula is as follows:
wherein the content of the first and second substances,is a nodeThe evaluation function of (a) the evaluation function of (b),the true cost value consumed by the original node to any node,is a nodeThe heuristic function of (2);
and generating a global path through the target navigation position, the grid map and the valuation function.
As will be appreciated, the amount of time required,is a variable fixed value, i.e. is the initial locationThe shortest distance value to the target location.
Specifically, the conventional a-algorithm is a heuristic path search method, and the general form of the evaluation function is:
whereinIs a nodeCost value consumed from the initial node to the target node; the actual cost value consumed from the initial node to any node;for robot slave nodeA heuristic of the cost value consumed moving to the target point.
In the embodiment, the heuristic function and the difference value between the initial position and the horizontal coordinate of the target navigation position are obtained by computing the heuristic function of the initial position, the initial movement angle and the target navigation position, the valuation function is obtained by computing the valuation function of the heuristic function in a first mode, and the global path is generated by the target navigation position, the grid map and the valuation function, so that the global path planned by the robot is closer to the real shortest path, and the requirements of global path optimization and real-time obstacle avoidance in the navigation process of the robot in the complex environment are effectively met.
Optionally, as an embodiment of the present invention, the original moving angle includes an original horizontal moving angle and an original vertical moving angle, and the calculating of the heuristic function on the initial position, the original moving angle, and the target navigation position to obtain the heuristic function and a difference between horizontal coordinates of the initial position and the target navigation position includes:
calculating a heuristic function of the initial position, the original horizontal movement included angle, the original vertical movement included angle and the target navigation position through a second formula to obtain the heuristic function and a difference value of horizontal coordinates of the initial position and the target navigation position, wherein the second formula is as follows:
wherein the content of the first and second substances,is a nodeThe heuristic function of (a) is,the difference value of the horizontal coordinates of the initial position and the target navigation position,the difference value of the vertical coordinates of the initial position and the target navigation position,the included angle is moved in the original horizontal direction,the included angle of the original vertical movement is obtained,is the abscissa of the target navigation position,is the abscissa of the initial position and is,is the ordinate of the target navigation position,is the ordinate of the initial position.
Specifically, the euclidean distance in equation (2) is used as an evaluation, and the path obtained by using this method is shortest, but the calculation amount is increased, so that the search efficiency is reduced, and the equation is as follows:
when the method is applied to scenes with high search efficiency requirements, the Manhattan distance in the formula (3) is selected as evaluation, and definition is performedAndrespectively representing the coordinates of the current point and the target point, and the evaluation formula is as follows:
to make a heuristic functionMore approximate to the true value (i.e. the distance between two straight lines), the invention combines the characteristics of the Manhattan distance and the Euclidean distance to set a new heuristic function, and the formula is as follows:
whereinThe included angle between the moving direction of the robot for avoiding the obstacle and the horizontal direction of the coordinate axis is shown,and the included angle between the motion direction of the robot for avoiding the obstacle and the vertical direction of the coordinate axis is shown. In the formula (4)Andrespectively representing the difference value of the abscissa and the ordinate between the current node position (i.e. the initial position) and the position of the target point (i.e. the target navigation position) of the robot, and the formula is as follows:
in the embodiment, the heuristic function and the difference value between the horizontal coordinates of the initial position and the target navigation position are obtained by calculating the heuristic function of the initial position, the original horizontal movement included angle, the original vertical movement included angle and the target navigation position through the second formula, so that the calculated amount is reduced, the searching efficiency is improved, and the capability of the mobile robot in avoiding dynamic obstacles and reaching a target point under a complex environment is realized.
Optionally, as an embodiment of the present invention, in step S4, a process of performing a path optimization analysis on the current movement angle, the horizontal coordinate difference between the initial position and the target navigation position, the global path, the target navigation position, the grid map, and the plurality of current distances according to the original movement angle to obtain an optimized path includes:
judging whether the current moving angle is equal to the original moving angle or not, and if so, taking the global path as an optimized path;
if not, obtaining a linear velocity value of the current sampling velocity of the robot through a speedometer arranged on the robot, and carrying out path optimization on the linear velocity value, the original horizontal movement included angle, the current movement angle, the target navigation position, the grid map and a difference value between the initial position and the horizontal coordinate of the target navigation position according to a plurality of current distances to obtain the optimized path.
It should be understood that, through the judgment of the current movement angle and the original movement angle, whether the path generates an offset or not is known, so that the globally optimal route can be more closely fitted.
In the embodiment, the optimized path is obtained by optimizing and analyzing the current movement angle, the horizontal coordinate difference value between the initial position and the target navigation position, the global path, the target navigation position, the grid map and paths of a plurality of current distances through the original movement angle, so that the global path optimization and real-time obstacle avoidance functions of the mobile robot navigation are realized, and certain effectiveness and feasibility are achieved.
Optionally, as an embodiment of the present invention, the current movement angle includes a current horizontal movement included angle, and the process of performing path optimization on the linear velocity value, the original horizontal movement included angle, the current movement angle, the target navigation position, the grid map, and a difference between horizontal coordinates of the initial position and the target navigation position according to a plurality of current distances includes:
judging whether the current distances are equal, if so, taking a first preset value as a value of an initial evaluation function; if not, screening the minimum value of the current distances to obtain the minimum distance, and taking the minimum distance as the value of the initial evaluation function;
calculating a target evaluation function for the linear velocity value, the original horizontal movement included angle, the initial evaluation function, the current horizontal movement included angle and the horizontal coordinate difference value between the initial position and the target navigation position by a third formula to obtain the target evaluation function, wherein the third formula is as follows:
wherein the content of the first and second substances,in order to be the objective evaluation function,to smooth the weights of the merit function,、、andare the weight values of the evaluation function,in order to evaluate the function for the azimuth,is an initial evaluation function, the value corresponding to the initial evaluation function is the first preset value or the minimum distance obtained by screening,the linear velocity value is the value of the linear velocity,in order to be a function of the distance evaluation,moving the included angle for the original horizontal direction,The difference value of the horizontal coordinates of the initial position and the target navigation position,the included angle is the current horizontal movement angle;
and generating an optimized path through the target navigation position, the grid map and the target evaluation function.
Preferably, the first preset value may be 0.
It should be understood that whether the current distances are all equal is judged, if yes, it is indicated that no obstacle exists in the area scanned by the two-dimensional laser radar, and therefore a first preset value is used as a value of an initial evaluation function;
if not, the fact that the obstacle exists in the area scanned by the two-dimensional laser radar is indicated, so that the minimum value of the current distances is screened, the minimum distance is obtained through screening, and the minimum distance is used as the value of the initial evaluation function.
Specifically, all motion trajectories are simulated for all the desirable sampling speed groups, and an optimal trajectory is selected by using an evaluation function method, wherein the evaluation function is as follows:
whereinThe method mainly comprises the following steps of as follows, for an azimuth angle evaluation function of a robot moving to a target point at a current sampling speed, assisting the robot to move towards a terminal point direction all the time in the moving process without deviating from the direction, and expressing:,i.e. the original horizontal directionMoving included angle;An evaluation function (namely the initial evaluation function) of the distance between the robot and the obstacle on the motion trail;the linear velocity value of the current sampling velocity of the robot is obtained;to smooth the weights of the merit function,、、is the weight of the evaluation function.
In order to enable the robot to be close to the original global path to a great extent and effectively avoid obstacles in the movement process, the invention adds a new distance evaluation function on the basis of the evaluation functionAnd an evaluation function representing the distance from the end point of the motion trail to the optimal path is as follows:
for the original planned path and the included angle of the coordinate axis in the horizontal direction (i.e. the original horizontal movement included angle)),The included angle between the motion direction of the robot avoiding the obstacle and the horizontal direction of the coordinate axis (namely the current horizontal moving included angle))。
In the above embodiment, the optimized path is obtained by optimizing a plurality of current distances to a linear velocity value, an original horizontal movement included angle, a current movement angle, a target navigation position, a grid map, and a path of a horizontal coordinate difference value between an initial position and a target navigation position, so that the robot can be close to an original global path to a great extent, and an obstacle can be effectively avoided in the movement process.
Alternatively, as another embodiment of the present invention, the speed of the robot needs to be limited within a certain range, there are a maximum speed and a minimum speed, and the formula is as follows:
the maximum increasing (decreasing) speed of the inspection robot is influenced by the performance of the motor, the simulated speed in the dynamic window must accord with the real speed, and the formula is as follows:
whereinIs the robot linear velocity;maximum linear acceleration;is the minimum linear acceleration;is the angular velocity of the robot;is the maximum angular acceleration;the minimum angular acceleration.
In order to ensure that the robot can stop before hitting the obstacle, the robot must keep a safe distance from the obstacle, and under the maximum speed constraint, the speed has a range, and the formula is as follows:
Optionally, as another embodiment of the present invention, the present invention uses upper computer software to control the robot to move, and obtains the raw data of the sensor through the two-dimensional laser radar carried by the mobile robot, and simulates the environment where the robot is located to build a map, which is represented as a grid map; setting a target navigation point on the grid map, and planning a global navigation path according to the position of an obstacle appearing on the path by using an A-star algorithm added with a new heuristic function; optimizing the local obstacle avoidance navigation path on the global navigation path planned by the improved A-x algorithm by utilizing a DWA algorithm added with a new distance evaluation function, so that the local obstacle avoidance navigation path is attached to the global navigation path planned by the improved A-x algorithm; and repeating the steps until the robot safely navigates to the target point according to the requirement of the robot for navigating a plurality of target points. The invention realizes the global path optimization and real-time obstacle avoidance functions of the mobile robot navigation, and has certain effectiveness and feasibility.
Optionally, as another embodiment of the invention, the invention optimizes the navigation algorithm of the mobile robot by improving the a-algorithm and the DWA algorithm to meet the requirements of global path optimization and real-time obstacle avoidance of the navigation process in the complex environment of the robot. Setting a new heuristic function for the A-algorithm to enable the global path planned by the robot to be closer to the real shortest path; on the basis of the traditional DWA algorithm, a new distance evaluation function is added, and in the safe speed range of the robot, some unnecessary moving directions are reduced to a certain extent, so that the moving track of the robot is smoother and can move to a target point more quickly, the improved A-x algorithm and the improved DWA algorithm realize the global path optimization and real-time obstacle avoidance functions of the mobile robot navigation, and have certain effectiveness and feasibility.
Optionally, as another embodiment of the present invention, the real value of the distance from the current position to the target point of the robot is set asWhen is coming into contact withIn time, the path search space is large, the number of nodes is large, the search efficiency is low, and the optimal solution can be searched finally; when in useIn the process, the path search space is small, the number of nodes is small, the search speed and efficiency are improved, but the planned path is not the optimal solution in most cases; when in useIn the process, the path search is expanded along the shortest path node sequence, so that the search efficiency is high, but the actual situation is difficult to realize. To make a heuristic functionMore approximate to the true valueThe invention combines the characteristics of the Manhattan distance and the Euclidean distance to set a new heuristic function, and the formula is as follows,the included angle between the moving direction of the robot for avoiding the obstacle and the horizontal direction of the coordinate axis is shown,and the included angle between the motion direction of the robot for avoiding the obstacle and the vertical direction of the coordinate axis is shown. In the formula (4)Andrespectively representing the difference values of the horizontal and vertical coordinates between the current node position of the robot and the position of the target point, and the formula is as follows:
in order to verify the effectiveness of the heuristic function, a special value is taken, and the included angle between the original motion direction of the robot to the target point and the horizontal direction of the coordinate axis is set under the condition that the robot has an obstacleThe robot avoids the included angle between the moving direction of the barrier and the horizontal direction of the coordinate axisLet the distance from A to CIn the right triangle ACB, the distance from C to B is obtained according to the cosine function, and the movement distance from a to B is 2.
According to heuristic functionsIs calculated to obtainThe true value of the original movement distance is 2, the heuristic function obtained through verification is closer to the true cost value, the global path optimization and real-time obstacle avoidance functions of the mobile robot navigation are realized, and certain effectiveness and feasibility are achieved.
Optionally, as another embodiment of the present invention, the executing steps of the present invention may further be:
s1, controlling the robot to move by using upper computer software, acquiring the original data of the sensor through a two-dimensional laser radar carried by the mobile robot, and simulating and drawing the environment where the robot is located to be represented as a grid map;
s2, setting target navigation points on the grid map, and planning a global navigation path according to the positions of the obstacles on the path by using an A-x algorithm added with a new heuristic function;
s3, optimizing the local obstacle avoidance navigation path on the planned global navigation path in S2 by using a DWA algorithm added with a new distance evaluation function, so that the local obstacle avoidance navigation path is attached to the planned global navigation path in S2;
and S4, repeating the S2 and S3 aiming at the requirement of the robot for navigating a plurality of target points until the robot safely navigates to the target points.
Fig. 2 is a block diagram of a robot navigation obstacle avoidance device according to an embodiment of the present invention.
Optionally, as another embodiment of the present invention, as shown in fig. 2, a robot navigation obstacle avoidance device includes:
the map building module is used for acquiring distances from emitted laser to a plurality of objects in a preset area through a two-dimensional laser radar arranged on the robot to obtain a plurality of original distances and building a grid map through the original distances;
the global path analysis module is used for leading in a target navigation position, obtaining an initial position through the two-dimensional laser radar, obtaining an original movement angle through a gyroscope sensor arranged on the robot, performing global path analysis on the initial position, the original movement angle, the target navigation position, the grid map and the target navigation position to obtain a global path and a horizontal coordinate difference value between the initial position and the target navigation position, and controlling the robot to move in the grid map along the global path;
the data acquisition module is used for acquiring the distances from the laser emitted by the current position of the robot to a plurality of objects through the two-dimensional laser radar to obtain a plurality of current distances and obtaining a current moving angle through the gyroscope sensor;
and the path optimization analysis module is used for performing path optimization analysis on the current movement angle, the horizontal coordinate difference value between the initial position and the target navigation position, the global path, the target navigation position, the grid map and the plurality of current distances according to the original movement angle to obtain an optimized path, controlling the robot to move in the grid map along the optimized path, and returning to the data acquisition module until the target navigation position is reached.
Optionally, as an embodiment of the present invention, the global path analysis module is specifically configured to:
calculating a heuristic function on the initial position, the original movement angle and the target navigation position to obtain the heuristic function and a horizontal coordinate difference value between the initial position and the target navigation position;
calculating an evaluation function through a first formula to the heuristic function to obtain the evaluation function, wherein the first formula is as follows:
wherein the content of the first and second substances,is a nodeThe evaluation function of (a) the evaluation function of (b),the true cost value consumed by the original node to any node,is a nodeThe heuristic function of (2);
and generating a global path through the target navigation position, the grid map and the valuation function.
Optionally, as an embodiment of the present invention, the original moving angle includes an original horizontal moving included angle and an original vertical moving included angle, and the process of calculating a heuristic function for the initial position, the original moving angle, and the target navigation position in the global path analysis module to obtain the heuristic function and a difference between horizontal coordinates of the initial position and the target navigation position includes:
calculating a heuristic function of the initial position, the original horizontal movement included angle, the original vertical movement included angle and the target navigation position through a second formula to obtain the heuristic function and a difference value of horizontal coordinates of the initial position and the target navigation position, wherein the second formula is as follows:
wherein the content of the first and second substances,is a nodeThe heuristic function of (a) is,the difference value of the horizontal coordinates of the initial position and the target navigation position,the difference value of the vertical coordinates of the initial position and the target navigation position,the included angle is moved in the original horizontal direction,the included angle of the original vertical movement is obtained,is the abscissa of the target navigation position,is the abscissa of the initial position and is,is the ordinate of the target navigation position,is the ordinate of the initial position.
Optionally, another embodiment of the present invention provides a robot navigation obstacle avoidance apparatus, including a memory, a processor, and a computer program stored in the memory and executable on the processor, and when the processor executes the computer program, the robot navigation obstacle avoidance method as described above is implemented. The device may be a computer or the like.
Optionally, another embodiment of the present invention provides a computer-readable storage medium, which stores a computer program, and when the computer program is executed by a processor, the method for robot navigation obstacle avoidance is implemented as described above.
It should be noted that, in this document, relational terms such as first and second, and the like are used solely to distinguish one entity or action from another entity or action without necessarily requiring or implying any actual such relationship or order between such entities or actions. Also, the terms "comprises," "comprising," or any other variation thereof, are intended to cover a non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements does not include only those elements but may include other elements not expressly listed or inherent to such process, method, article, or apparatus.
It is clear to those skilled in the art that, for convenience and brevity of description, the specific working processes of the above-described apparatuses and units may refer to the corresponding processes in the foregoing method embodiments, and are not described herein again.
In the several embodiments provided in the present application, it should be understood that the disclosed apparatus and method may be implemented in other manners. For example, the above-described apparatus embodiments are merely illustrative, and for example, a division of a unit is merely a logical division, and an actual implementation may have another division, for example, a plurality of units or components may be combined or integrated into another system, or some features may be omitted, or not executed.
Units described as separate parts may or may not be physically separate, and parts displayed as units may or may not be physical units, may be located in one place, or may be distributed on a plurality of network units. Some or all of the units can be selected according to actual needs to achieve the purpose of the solution of the embodiments of the present invention.
In addition, functional units in the embodiments of the present invention may be integrated into one processing unit, or each unit may exist alone physically, or two or more units are integrated into one unit. The integrated unit can be realized in a form of hardware, and can also be realized in a form of a software functional unit.
The integrated unit, if implemented in the form of a software functional unit and sold or used as a stand-alone product, may be stored in a computer readable storage medium. Based on such understanding, the technical solution of the present invention essentially or partially contributes to the prior art, or all or part of the technical solution can be embodied in the form of a software product stored in a storage medium and including instructions for causing a computer device (which may be a personal computer, a server, or a network device) to execute all or part of the steps of the method according to the embodiments of the present invention. And the aforementioned storage medium includes: a U-disk, a removable hard disk, a Read-Only Memory (ROM), a Random Access Memory (RAM), a magnetic disk, or an optical disk, and various media capable of storing program codes.
The above description is only for the purpose of illustrating the preferred embodiments of the present invention and is not to be construed as limiting the invention, and any modifications, equivalents, improvements and the like that fall within the spirit and principle of the present invention are intended to be included therein.
Claims (10)
1. A robot navigation obstacle avoidance method is characterized by comprising the following steps:
s1: acquiring distances from emitted laser to a plurality of objects in a preset area through a two-dimensional laser radar arranged on the robot to obtain a plurality of original distances, and constructing a grid map through the plurality of original distances;
s2: a target navigation position is led in, an initial position is obtained through the two-dimensional laser radar, an original moving angle is obtained through a gyroscope sensor arranged on the robot, global path analysis is carried out on the initial position, the original moving angle, the target navigation position, the grid map and the target navigation position, a global path and a horizontal coordinate difference value between the initial position and the target navigation position are obtained, and the robot is controlled to move in the grid map along the global path;
s3: acquiring distances from the laser emitted by the current position of the robot to a plurality of objects through the two-dimensional laser radar to obtain a plurality of current distances, and obtaining a current moving angle through the gyroscope sensor;
s4: and performing path optimization analysis on the current movement angle, the horizontal coordinate difference value between the initial position and the target navigation position, the global path, the target navigation position, the grid map and the current distances according to the original movement angle to obtain an optimized path, controlling the robot to move in the grid map along the optimized path, and returning to the step S3 until the target navigation position is reached.
2. The robot navigation obstacle avoidance method according to claim 1, wherein in step S2, the process of performing global path analysis on the initial position, the initial movement angle, the target navigation position, the grid map, and the target navigation position to obtain a global path and a difference between horizontal coordinates of the initial position and the target navigation position includes:
calculating a heuristic function on the initial position, the original movement angle and the target navigation position to obtain the heuristic function and a horizontal coordinate difference value between the initial position and the target navigation position;
calculating an evaluation function through a first formula to the heuristic function to obtain the evaluation function, wherein the first formula is as follows:
wherein the content of the first and second substances,is a nodeThe evaluation function of (a) the evaluation function of (b),the true cost value consumed by the original node to any node,is a nodeThe heuristic function of (2);
and generating a global path through the target navigation position, the grid map and the valuation function.
3. The robot navigation obstacle avoidance method according to claim 2, wherein the original movement angle includes an original horizontal movement angle and an original vertical movement angle, and the process of calculating the initial position, the original movement angle and the target navigation position by the heuristic function to obtain the heuristic function and the difference between the horizontal coordinates of the initial position and the target navigation position includes:
calculating a heuristic function of the initial position, the original horizontal movement included angle, the original vertical movement included angle and the target navigation position through a second formula to obtain the heuristic function and a difference value of horizontal coordinates of the initial position and the target navigation position, wherein the second formula is as follows:
wherein the content of the first and second substances,is a nodeThe heuristic function of (a) is,the difference value of the horizontal coordinates of the initial position and the target navigation position,is the difference value of the initial position and the target navigation position vertical coordinate,the included angle is moved in the original horizontal direction,the included angle of the original vertical movement is obtained,is the abscissa of the target navigation position,is the abscissa of the initial position and is,is the ordinate of the target navigation position,is the ordinate of the initial position.
4. The robot navigation obstacle avoidance method according to claim 3, wherein in step S4, the process of performing path optimization analysis on the current movement angle, the difference between the horizontal coordinates of the initial position and the target navigation position, the global path, the target navigation position, the grid map, and the plurality of current distances according to the original movement angle, and obtaining the optimized path includes:
judging whether the current moving angle is equal to the original moving angle or not, and if so, taking the global path as an optimized path;
if not, obtaining a linear velocity value of the current sampling velocity of the robot through a speedometer arranged on the robot, and carrying out path optimization on the linear velocity value, the original horizontal movement included angle, the current movement angle, the target navigation position, the grid map and a difference value between the initial position and the horizontal coordinate of the target navigation position according to a plurality of current distances to obtain the optimized path.
5. The robot navigation obstacle avoidance method according to claim 4, wherein the current movement angle includes a current horizontal movement angle, and the process of performing path optimization on the linear velocity value, the original horizontal movement angle, the current movement angle, the target navigation position, the grid map, and the difference between the horizontal coordinates of the initial position and the target navigation position according to the plurality of current distances includes:
judging whether the current distances are equal, if so, taking a first preset value as a value of an initial evaluation function; if not, screening the minimum value of the current distances to obtain the minimum distance, and taking the minimum distance as the value of the initial evaluation function;
calculating a target evaluation function according to a third formula on the linear velocity value, the original horizontal movement included angle, the initial evaluation function, the current horizontal movement included angle and the horizontal coordinate difference value between the initial position and the target navigation position to obtain the target evaluation function, wherein the third formula is as follows:
wherein the content of the first and second substances,in order to be the objective evaluation function,to smooth the weights of the evaluation function,、、andare the weight values of the evaluation function,in order to evaluate the function for the azimuth,is an initial evaluation function, the value corresponding to the initial evaluation function is the first preset value or the minimum distance obtained by screening,the linear velocity value is the value of the linear velocity,in order to be a function of the distance evaluation,the included angle is moved in the original horizontal direction,the difference value of the horizontal coordinates of the initial position and the target navigation position,the included angle is the current horizontal movement angle;
and generating an optimized path through the target navigation position, the grid map and the target evaluation function.
6. The utility model provides a barrier device is kept away in navigation of robot which characterized in that includes:
the map building module is used for acquiring distances from emitted laser to a plurality of objects in a preset area through a two-dimensional laser radar arranged on the robot to obtain a plurality of original distances and building a grid map through the plurality of original distances;
the global path analysis module is used for leading in a target navigation position, obtaining an initial position through the two-dimensional laser radar, obtaining an original movement angle through a gyroscope sensor arranged on the robot, performing global path analysis on the initial position, the original movement angle, the target navigation position, the grid map and the target navigation position to obtain a global path and a horizontal coordinate difference value between the initial position and the target navigation position, and controlling the robot to move in the grid map along the global path;
the data acquisition module is used for acquiring the distances from the laser emitted by the current position of the robot to a plurality of objects through the two-dimensional laser radar to obtain a plurality of current distances and obtaining a current moving angle through the gyroscope sensor;
and the path optimization analysis module is used for performing path optimization analysis on the current movement angle, the horizontal coordinate difference value between the initial position and the target navigation position, the global path, the target navigation position, the grid map and the plurality of current distances according to the original movement angle to obtain an optimized path, controlling the robot to move in the grid map along the optimized path, and returning to the data acquisition module until the target navigation position is reached.
7. The robot navigation obstacle avoidance device of claim 6, wherein the global path analysis module is specifically configured to:
calculating a heuristic function on the initial position, the original movement angle and the target navigation position to obtain the heuristic function and a horizontal coordinate difference value between the initial position and the target navigation position;
calculating an evaluation function through a first formula to the heuristic function to obtain the evaluation function, wherein the first formula is as follows:
wherein the content of the first and second substances,is a nodeThe evaluation function of (a) the evaluation function of (b),the true cost value consumed by the original node to any node,a heuristic function for a node;
and generating a global path through the target navigation position, the grid map and the valuation function.
8. The robot navigation obstacle avoidance device of claim 7, wherein the original movement angle includes an original horizontal movement angle and an original vertical movement angle, and the process of calculating the heuristic function for the initial position, the original movement angle, and the target navigation position in the global path analysis module to obtain the heuristic function and the difference between the horizontal coordinates of the initial position and the target navigation position includes:
calculating a heuristic function of the initial position, the original horizontal movement included angle, the original vertical movement included angle and the target navigation position through a second formula to obtain the heuristic function and a difference value of horizontal coordinates of the initial position and the target navigation position, wherein the second formula is as follows:
wherein the content of the first and second substances,is a nodeThe heuristic function of (a) is,the difference value of the horizontal coordinates of the initial position and the target navigation position,the difference value of the vertical coordinates of the initial position and the target navigation position,the included angle is moved in the original horizontal direction,the included angle of the original vertical movement is obtained,is the abscissa of the target navigation position,is the abscissa of the initial position and is,is the ordinate of the target navigation position,is the ordinate of the initial position.
9. A robot navigation obstacle avoidance system comprising a memory, a processor and a computer program stored in the memory and executable on the processor, characterized in that when the processor executes the computer program, the robot navigation obstacle avoidance method according to any one of claims 1 to 5 is implemented.
10. A computer-readable storage medium storing a computer program, wherein the computer program, when executed by a processor, implements the robot navigation obstacle avoidance method according to any one of claims 1 to 5.
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210944226.1A CN115016510A (en) | 2022-08-08 | 2022-08-08 | Robot navigation obstacle avoidance method and device and storage medium |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210944226.1A CN115016510A (en) | 2022-08-08 | 2022-08-08 | Robot navigation obstacle avoidance method and device and storage medium |
Publications (1)
Publication Number | Publication Date |
---|---|
CN115016510A true CN115016510A (en) | 2022-09-06 |
Family
ID=83066030
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202210944226.1A Pending CN115016510A (en) | 2022-08-08 | 2022-08-08 | Robot navigation obstacle avoidance method and device and storage medium |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN115016510A (en) |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN117055585A (en) * | 2023-10-09 | 2023-11-14 | 青州市巨龙环保科技有限公司 | Intelligent control method and system for intelligent underwater robot |
Citations (11)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20190094866A1 (en) * | 2017-09-22 | 2019-03-28 | Locus Robotics Corporation | Dynamic window approach using optimal reciprocal collision avoidance cost-critic |
WO2019190395A1 (en) * | 2018-03-28 | 2019-10-03 | Agency For Science, Technology And Research | Method and system for returning a displaced autonomous mobile robot to its navigational path |
CN112066989A (en) * | 2020-08-19 | 2020-12-11 | 合肥工业大学 | Indoor AGV automatic navigation system and method based on laser SLAM |
CN112325884A (en) * | 2020-10-29 | 2021-02-05 | 广西科技大学 | ROS robot local path planning method based on DWA |
CN112506199A (en) * | 2020-12-12 | 2021-03-16 | 江西洪都航空工业集团有限责任公司 | Local path planning method based on dynamic window method and suitable for Ackerman model robot |
CN112731916A (en) * | 2020-10-22 | 2021-04-30 | 福建工程学院 | Global dynamic path planning method integrating skip point search method and dynamic window method |
CN113341984A (en) * | 2021-06-15 | 2021-09-03 | 桂林电子科技大学 | Robot path planning method and device based on improved RRT algorithm |
CN114428499A (en) * | 2021-12-16 | 2022-05-03 | 哈尔滨理工大学 | Astar and DWA algorithm fused mobile trolley path planning method |
CN114510057A (en) * | 2022-02-21 | 2022-05-17 | 沈阳理工大学 | ROS-based mobile robot autonomous navigation method in indoor environment |
CN114625150A (en) * | 2022-05-17 | 2022-06-14 | 南京汇与信息科技有限公司 | Rapid ant colony unmanned ship dynamic obstacle avoidance method based on danger index and distance function |
CN114859929A (en) * | 2022-05-19 | 2022-08-05 | 哈尔滨工业大学(威海) | AGV path planning method based on improved DWA algorithm in dynamic environment |
-
2022
- 2022-08-08 CN CN202210944226.1A patent/CN115016510A/en active Pending
Patent Citations (11)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20190094866A1 (en) * | 2017-09-22 | 2019-03-28 | Locus Robotics Corporation | Dynamic window approach using optimal reciprocal collision avoidance cost-critic |
WO2019190395A1 (en) * | 2018-03-28 | 2019-10-03 | Agency For Science, Technology And Research | Method and system for returning a displaced autonomous mobile robot to its navigational path |
CN112066989A (en) * | 2020-08-19 | 2020-12-11 | 合肥工业大学 | Indoor AGV automatic navigation system and method based on laser SLAM |
CN112731916A (en) * | 2020-10-22 | 2021-04-30 | 福建工程学院 | Global dynamic path planning method integrating skip point search method and dynamic window method |
CN112325884A (en) * | 2020-10-29 | 2021-02-05 | 广西科技大学 | ROS robot local path planning method based on DWA |
CN112506199A (en) * | 2020-12-12 | 2021-03-16 | 江西洪都航空工业集团有限责任公司 | Local path planning method based on dynamic window method and suitable for Ackerman model robot |
CN113341984A (en) * | 2021-06-15 | 2021-09-03 | 桂林电子科技大学 | Robot path planning method and device based on improved RRT algorithm |
CN114428499A (en) * | 2021-12-16 | 2022-05-03 | 哈尔滨理工大学 | Astar and DWA algorithm fused mobile trolley path planning method |
CN114510057A (en) * | 2022-02-21 | 2022-05-17 | 沈阳理工大学 | ROS-based mobile robot autonomous navigation method in indoor environment |
CN114625150A (en) * | 2022-05-17 | 2022-06-14 | 南京汇与信息科技有限公司 | Rapid ant colony unmanned ship dynamic obstacle avoidance method based on danger index and distance function |
CN114859929A (en) * | 2022-05-19 | 2022-08-05 | 哈尔滨工业大学(威海) | AGV path planning method based on improved DWA algorithm in dynamic environment |
Non-Patent Citations (2)
Title |
---|
叶泽团: "基于ROS的配电室巡检机器人路径规划的研究", 《中国优秀硕士学位论文全文数据库(电子期刊)》 * |
庞永旭 等: "融合改进A∗与DWA算法的移动机器人路径规划", 《计算机与现代化》 * |
Cited By (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN117055585A (en) * | 2023-10-09 | 2023-11-14 | 青州市巨龙环保科技有限公司 | Intelligent control method and system for intelligent underwater robot |
CN117055585B (en) * | 2023-10-09 | 2024-03-05 | 青州市巨龙环保科技有限公司 | Intelligent control method and system for intelligent underwater robot |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN109782763B (en) | Mobile robot path planning method in dynamic environment | |
CN110645974B (en) | Mobile robot indoor map construction method fusing multiple sensors | |
CN113110457B (en) | Autonomous coverage inspection method for intelligent robot in indoor complex dynamic environment | |
CN109597425B (en) | Unmanned aerial vehicle navigation and obstacle avoidance method based on reinforcement learning | |
CN112880694B (en) | Method for determining the position of a vehicle | |
AU2005278160A1 (en) | System and method for adaptive path planning | |
CN112284376A (en) | Mobile robot indoor positioning mapping method based on multi-sensor fusion | |
Hanten et al. | Vector-AMCL: Vector based adaptive monte carlo localization for indoor maps | |
CN112712193A (en) | Multi-unmanned aerial vehicle local route planning method and device based on improved Q-Learning | |
CN108628318A (en) | Congestion environment detection method, device, robot and storage medium | |
Tang et al. | Robot tracking in SLAM with Masreliez-Martin unscented Kalman filter | |
WO2020008755A1 (en) | Information processing device, information processing system, action planning method, and program | |
Wettach et al. | Dynamic frontier based exploration with a mobile indoor robot | |
CN114879660B (en) | Robot environment sensing method based on target drive | |
CN115016510A (en) | Robot navigation obstacle avoidance method and device and storage medium | |
CN114608585A (en) | Method and device for synchronous positioning and mapping of mobile robot | |
JP7058761B2 (en) | Mobile control device, mobile control learning device, and mobile control method | |
Tolt et al. | Multi-aspect path planning for enhanced ground combat simulation | |
CN116576868A (en) | Multi-sensor fusion accurate positioning and autonomous navigation method | |
CN112904855B (en) | Follow-up robot local path planning method based on improved dynamic window | |
Lagoudakis et al. | Neural maps for mobile robot navigation | |
Stamford et al. | Pathfinding in partially explored games environments: The application of the A* Algorithm with occupancy grids in Unity3D | |
Bacha et al. | A new robust cooperative-reactive Filter for vehicle localization: The Extended Kalman Particle Swarm ‘EKPS’ | |
Hellstrom et al. | Real-time path planning using a simulator-in-the-loop | |
Kudriashov et al. | Introduction to Mobile Robots Navigation, Localization and Mapping |
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 |
Application publication date: 20220906 |
|
RJ01 | Rejection of invention patent application after publication |