CN116592887A - Accurate positioning navigation algorithm of indoor mobile robot with smooth track gradient - Google Patents
Accurate positioning navigation algorithm of indoor mobile robot with smooth track gradient Download PDFInfo
- Publication number
- CN116592887A CN116592887A CN202310491190.0A CN202310491190A CN116592887A CN 116592887 A CN116592887 A CN 116592887A CN 202310491190 A CN202310491190 A CN 202310491190A CN 116592887 A CN116592887 A CN 116592887A
- Authority
- CN
- China
- Prior art keywords
- map
- robot
- grid
- algorithm
- module
- 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
- 238000004422 calculation algorithm Methods 0.000 title claims abstract description 66
- 238000012423 maintenance Methods 0.000 claims abstract description 24
- 238000013507 mapping Methods 0.000 claims abstract description 11
- 238000005516 engineering process Methods 0.000 claims abstract description 8
- 239000002245 particle Substances 0.000 claims description 24
- 238000005381 potential energy Methods 0.000 claims description 23
- 238000000034 method Methods 0.000 claims description 14
- 230000008569 process Effects 0.000 claims description 11
- 230000001133 acceleration Effects 0.000 claims description 7
- 238000001914 filtration Methods 0.000 claims description 6
- 238000005259 measurement Methods 0.000 claims description 6
- 238000004088 simulation Methods 0.000 claims description 6
- 230000004888 barrier function Effects 0.000 claims description 4
- 230000001360 synchronised effect Effects 0.000 claims description 4
- 238000012952 Resampling Methods 0.000 claims description 3
- 238000004364 calculation method Methods 0.000 claims description 3
- 239000011159 matrix material Substances 0.000 claims description 3
- 238000010606 normalization Methods 0.000 claims description 3
- 230000009471 action Effects 0.000 description 3
- 238000010586 diagram Methods 0.000 description 3
- 235000012054 meals Nutrition 0.000 description 3
- 238000004140 cleaning Methods 0.000 description 2
- 230000008878 coupling Effects 0.000 description 2
- 238000010168 coupling process Methods 0.000 description 2
- 238000005859 coupling reaction Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 238000010408 sweeping Methods 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 239000000428 dust Substances 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 230000004044 response Effects 0.000 description 1
- 238000005406 washing Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
- G01C21/206—Instruments for performing navigational calculations specially adapted for indoor navigation
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/38—Electronic maps specially adapted for navigation; Updating thereof
- G01C21/3804—Creation or updating of map data
- G01C21/3807—Creation or updating of map data characterised by the type of data
- G01C21/383—Indoor data
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/38—Electronic maps specially adapted for navigation; Updating thereof
- G01C21/3804—Creation or updating of map data
- G01C21/3833—Creation or updating of map data characterised by the source of data
- G01C21/3837—Data obtained from a single source
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The invention discloses an indoor mobile robot accurate positioning navigation algorithm with smooth track gradient, which adopts a navigation system, the system comprises a map maintenance module, a global planner module, a positioning module and a local planner module, the sub-algorithm of the map maintenance module carries out online maintenance and updating on a map based on a grid map constructed by a pre-used positioning and mapping technology and real-time sensing information of a laser radar sensor, the global planner module is responsible for planning an obstacle-free path from a starting point to an end point for a robot based on the latest map information provided by the map maintenance module, the positioning module calculates the gesture of the robot in a map coordinate system based on the map information by utilizing environment data observed by the laser radar sensor, and the local planner module is responsible for following the path from the global planner.
Description
Technical Field
The invention relates to the technical field of navigation algorithms, in particular to an indoor mobile robot accurate positioning navigation algorithm with smooth track gradient.
Background
Indoor mobile robots are being widely popularized, and are a popular robotics product. Such as a sweeping robot, a service robot, a meal delivery robot and the like, which can replace human labor in various scenes, for example, the sweeping robot can bear most of the work of cleaning the household environment, not only can sweep the floor and suck dust, but also can finish cleaning tasks such as mopping, washing and the like. The service robot may help guests at the mall find a desired place. The meal delivery robot can replace a restaurant attendant to carry out meal delivery work.
For indoor mobile robots, the navigation function is the basis for the indoor mobile robots to complete various application functions, so that the working efficiency and the life quality of people can be greatly improved. However, the running path and the local track of the robot drawn by the navigation algorithm of the existing indoor mobile robot are not smooth enough. Due to the inherent motor response characteristics of the robot, if the robot control command is not smooth enough, frequent and large-amplitude speed changes will reduce the navigation efficiency of the robot, and meanwhile frequent acceleration and deceleration will reduce the service life of the motor. In addition, the navigation algorithm of the existing indoor mobile robot generally only considers the parts of global path planning and local path planning independently, does not consider the relation between the navigation algorithm and map real-time maintenance updating and positioning, and often exists in a loose coupling relation with the map real-time maintenance updating and positioning, and the relation is not tight enough, so that the planning effect is not accurate enough. Therefore, it is necessary to design an indoor mobile robot accurate positioning navigation algorithm with accurate track gradient.
Disclosure of Invention
The invention aims to provide an indoor mobile robot accurate positioning navigation algorithm with smooth track gradient so as to solve the problems in the background technology.
In order to solve the technical problems, the invention provides the following technical scheme: the navigation algorithm adopts a navigation system, the system comprises a map maintenance module, a global planner module, a positioning module and a local planner module, the sub-algorithm of the map maintenance module is used for carrying out online maintenance and updating on a map based on a grid map constructed by a pre-used positioning and mapping technology and real-time sensing information of a laser radar sensor, the global planner module is used for planning an obstacle-free path from a starting point to an end point for the robot based on the latest map information provided by the map maintenance module, the positioning module is used for calculating the pose of the robot in a map coordinate system based on the map information by utilizing environment data observed by the laser radar sensor and comprises a position coordinate and an orientation angle, and the local planner module is used for following the path from the global planner and providing pose information for feedback by utilizing the positioning module, so that accurate path following and obstacle avoidance are realized.
According to the technical scheme, the map maintenance module part is used for carrying out maintenance on the basis of a grid map which is built by a user in advance by using a synchronous positioning and mapping technology and is divided into a global map and a local map, wherein the grid map is used for dividing a plane map into a plurality of cells by a square with a certain resolution, namely, the map is discretized into a two-dimensional matrix, each cell is called a grid, the state of the grid is represented by different grid values, the value represents the probability that the grid is an obstacle, the probability is 0, the probability is represented by a white grid of a passable area, the probability is 1, the probability is represented by a black grid of an obstacle, and the probability is between 0 and 1, and represents the unexplored area of the robot;
based on the original grid map, the algorithm represents the global map and the local map in the form of a cost map, namely, the cost is generally represented by an integer between 0 and 255, in the algorithm, the distance between the barrier of the grid and the geometric center of the robot is represented by a value between 128 and 255, the greater the probability of collision is represented by greater, and the distance between the grid and the outermost contour of the robot is represented by 0 to 128;
the global map maintained by the algorithm is a cost map of the same size as the original grid map, while the local map is a square area with a size centered on the robot, the resolution of the local map is higher than that of the global map, and when a sensor such as a laser radar installed on the robot finds a new obstacle in the environment, the information is updated to the grid map first, and then to the global map and the local map.
According to the above technical solution, the algorithm of updating the grid map is as follows, for one grid in the map, the probability that the grid is an obstacle is represented by P (state=1), the probability that the grid is a passable area is represented by P (state=0), and the two probabilities satisfy:
P(state＝1)+P(state＝0)＝1
the ratio of these two probabilities is noted as:
in addition, the current observation data of the laser radar sensor on the environment is z, and the probability that the grid is an obstacle and a passable area after z is given is respectively as follows:
the ratio of the two is:
the two sides take the logarithm:
of which onlyThe term comprising the measured values, the log ratio being referred to as the measurement model, the value of which is related to the actual measurement, depending on whether the rays of the lidar hit the current grid, assuming that a grid is a barrier in the future before any sensor observations beginThe probability of an obstacle or passable area is equal, the initial value of log q (state) is:
according to the technical scheme, the global planner module can plan a smooth global path based on a Dijkstra path planning algorithm with gradient descent, the Dijkstra algorithm can find the shortest path of the grid from the starting point grid to the end point, a step of calculating the potential energy value of the grid is added in the process of finding the shortest path by the Dijkstra algorithm in the planning process of the algorithm, namely, the grid near the shortest path updates a potential energy value, then starts from the starting point grid, calculates potential energy gradients of 8 directions according to the potential energy values of 8 neighbor grids near the grid, acquires the next path point according to a set step length along the direction with the maximum potential energy gradient, and then starts from the next path point, continues to calculate the next path point along the direction with the maximum potential energy gradient until reaching the vicinity of the starting point, and the path is not a fixed grid length distance, so that the path is a smooth curve, wherein the potential energy interpolation function is performed by the Quadrastic interpolation function on the basis of the current grid on the left and the right of the current grid.
According to the technical proposal, the positioning module uses a particle filtering algorithm based on a Bayesian filtering framework to position based on the occupied grid map and the current laser radar data,
the positioning process comprises the following main steps:
(1) Randomly generating N particles, wherein each particle represents a probability hypothesis for the pose of the robot in the map;
(2) Calculating the weight of each particle according to the latest observed laser radar data, wherein the weight of the position of the particle is larger in accordance with the observed data, and otherwise, the weight is smaller;
(3) And (3) forming a new set by the particles with the weight values, resampling according to the weight values to obtain a new particle set, wherein the weight is the largest in the new particle set and is used for representing the current pose of the robot.
According to the above technical solution, the local planner module sets the maximum acceleration of the robot as a, the maximum angular acceleration as β, the simulation time of each planning as Δt, and the current linear velocity of the robot as v, based on the solution of the loss function of the robot hard constraint now The current angular velocity is ω now For the robot, after Δt time, the range of values of the linear velocity and the angular velocity is:
v∈[v now -a·Δt,v now +a·Δt]
ω∈[ω now -β·Δt,ω now +β·Δt]
in addition, the values considering the theoretical maximum linear velocity and angular velocity of the robot are: v max And omega max In the safety consideration, and the robot is not allowed to have backward, namely v is more than or equal to 0, the range of values of the linear speed and the angular speed of the robot is as follows:
v∈[max(0,v now -a·Δt),min(v now +a·Δt,v max )]
ω∈[max(-ω max ,ω now -β·Δt),min(ω now +β·Δt,ω max )]。
the algorithm of the local planning comprises the following steps:
(6) The linear speed and angular speed range of the robot are respectively discretized into M values and N values, wherein M and N are manually preset values, and then are arranged and combined into M multiplied by N (linear speed and angular speed) value combinations;
(7) For each combination of M×N (v, ω), the robot trajectory is calculated by simulation while keeping unchanged in Δt time, and the robot trajectory is a circular arc, and the current pose of the robot is recorded as (x) now ,y now ,θ now ) The trajectory of the robot after Δt time is:
θ t ＝θ now +ωΔt
(8) Calculating and simulating the track of the robot and the nearest distance d of the obstacle, judging whether collision occurs with the obstacle in the map, and eliminating the combination of collision to obtain a residual track set T;
(9) For the tracks in the set T, firstly, carrying out normalization calculation on the speeds (v, omega), the nearest distance d to the obstacle and the attitude angle theta in the set, and mapping the values of the speeds (v, omega), the nearest distance d to the obstacle and the attitude angle theta to be between [0,1 ];
the speed of the minimum track of the cost in the T is calculated according to a series of costs and is taken as the final control instruction of the robot.
According to the technical scheme, the cost is defined as follows:
TotalCost＝GoalCost+ObstacleCost+VelocityCost
GoalCost＝distance(Goal,RobotPos)
ObstacleCost＝-min_distanace_to_obstacle
VelocityCost＝α·v+β·ω
the Goalcost is the Euclidean distance from the current position RobotPos of the robot to the target point Goal, the obstaclecast is used for measuring the distance from the robot to the obstacle, and the VelocityCost is a punishment item for the linear speed and the angular speed.
Compared with the prior art, the invention has the following beneficial effects: according to the invention, a tight coupling algorithm is adopted, and the positioning module is utilized to provide pose information for feedback, so that accurate path following and obstacle avoidance are realized, and as the distance from each path point to the next path point is related to potential energy gradient and is not a fixed grid length distance, the planned path is a smooth curve.
Drawings
The accompanying drawings are included to provide a further understanding of the invention and are incorporated in and constitute a part of this specification, illustrate the invention and together with the embodiments of the invention, serve to explain the invention. In the drawings:
FIG. 1 is a schematic view of the overall modular structure of the present invention;
FIG. 2 is a schematic diagram of a grid map constructed by the synchronous positioning and mapping technique of the present invention;
FIG. 3 is an algorithmic schematic of the global planner module of the present invention;
FIG. 4 is a schematic diagram of a candidate set of local trajectories for a global planning algorithm of the present invention;
FIG. 5 is a schematic diagram of a positioning module algorithm of the present invention.
Detailed Description
The following description of the embodiments of the present invention will be made clearly and completely with reference to the accompanying drawings, in which it is apparent that the embodiments described are only some embodiments of the present invention, but not all embodiments. All other embodiments, which can be made by those skilled in the art based on the embodiments of the invention without making any inventive effort, are intended to be within the scope of the invention.
Referring to fig. 1-5, the present invention provides the following technical solutions: the navigation algorithm adopts a navigation system, the system comprises a map maintenance module, a global planner module, a positioning module and a local planner module, the sub-algorithm of the map maintenance module carries out online maintenance and update on a map based on the real-time sensing information of a grid map and a laser radar sensor constructed by a pre-used positioning and mapping technology, the global planner module is responsible for planning an obstacle-free path from a starting point to an end point for the robot based on the latest map information provided by the map maintenance module, the positioning module calculates the attitude of the robot in a map coordinate system based on the map information by utilizing environment data observed by the laser radar sensor, the attitude coordinate and an orientation angle are included, and the local planner module is responsible for following the path from the global planner and providing pose information for feedback by utilizing the positioning module, so that accurate path following and obstacle avoidance are realized;
the map maintenance module part is used for carrying out maintenance by dividing a grid map which is constructed by a user in advance by using a synchronous positioning and mapping technology into a global map and a local map, wherein the grid map is used for dividing a plane map into a plurality of cells by a square with a certain resolution, namely the map is discretized into a two-dimensional matrix, each cell is called a grid, the state of the grid is represented by different grid values, the value represents the probability that the grid is an obstacle, the probability is 0, the probability is represented by a passable area and is represented by a white grid of the graph, the probability is 1, the probability is represented by a black grid of the obstacle, and the probability is between 0 and 1, and represents the unexplored area of the robot;
based on the original grid map, the algorithm represents the global map and the local map in the form of a cost map, namely, the cost is generally represented by an integer between 0 and 255, in the algorithm, the distance between the barrier of the grid and the geometric center of the robot is represented by a value between 128 and 255, the greater the probability of collision is represented by greater, and the distance between the grid and the outermost contour of the robot is represented by 0 to 128;
the global map maintained by the algorithm is a cost map with the same size as the original grid map, the local map is a square area cost map with a size centered by the robot, the resolution of the local map is higher than that of the global map, and when a sensor such as a laser radar installed on the robot finds a new obstacle in the environment, the information is updated into the grid map firstly and then into the global map and the local map;
the algorithm of the grid map update is as follows, for one grid in the map, the probability that the grid is an obstacle is represented by P (state=1), the probability that the grid is a passable area is represented by P (state=0), and the two probabilities satisfy:
P(state＝1)+P(state＝0)＝1
the ratio of these two probabilities is noted as:
in addition, the current observation data of the laser radar sensor on the environment is z, and the probability that the grid is an obstacle and a passable area after z is given is respectively as follows:
the ratio of the two is:
the two sides take the logarithm:
of which onlyThe term contains the measured value, this logarithmic ratio is called the measurement model, and this value is related to the actual measurement situation, depending on whether the rays of the lidar hit the current grid, assuming that the probability of a grid being an obstacle or passable area in the future before any sensor observation is started is equal, the initial value of log q (state) is:
the global planner module can plan a smooth global path based on a Dijkstra path planning algorithm of gradient descent, the Dijkstra algorithm can find the shortest path of a grid from a starting point grid path to an end point, a step of calculating potential energy value of a grid is added in the process of finding the shortest path by the Dijkstra algorithm in the planning process of the algorithm, namely, the grid near the shortest path updates a potential energy value, then starts from the starting point grid, calculates potential energy gradients in 8 directions according to potential energy values of 8 neighbor grids near the grid, acquires the next path point along the direction with the largest potential energy gradient according to a set step length, then starts from the next path point, continues to calculate the next path point along the direction with the largest potential energy gradient until the next path point reaches the vicinity of the starting point, and is not a fixed grid length distance, so the planned path is a smooth curve, a quadric interpolation function is used as the potential energy interpolation function, and the current function is used for interpolating the current grid on the basis of the left and the right neighbor grid;
the positioning module uses a particle filtering algorithm based on a Bayesian filtering framework to position based on the occupancy grid map and the current lidar data,
the positioning process comprises the following main steps:
(1) Randomly generating N particles, wherein each particle represents a probability hypothesis for the pose of the robot in the map;
(2) Calculating the weight of each particle according to the latest observed laser radar data, wherein the weight of the position of the particle is larger in accordance with the observed data, and otherwise, the weight is smaller;
(3) The particles with the weight values form a new set, the new set of particles is obtained by resampling according to the weight values, and the weight is the largest in the new set of particles and is used for representing the current pose of the robot;
the local planner module is based on solving of a loss function of robot hard constraint, and the maximum acceleration of the robot is set as a, the maximum angular acceleration is set as beta, simulation time of each planning is set as deltat, and the current linear speed of the robot is set as v now The current angular velocity is ω now For the robot, after Δt time, the range of values of the linear velocity and the angular velocity is:
v∈[v now -a·Δt,v now +a·Δt]
ω∈[ω now -β·Δt,ω now +β·Δt]
in addition, the values considering the theoretical maximum linear velocity and angular velocity of the robot are: v max And omega max In the safety consideration, and the robot is not allowed to have backward, namely v is more than or equal to 0, the range of values of the linear speed and the angular speed of the robot is as follows:
v∈[max(0,v now -a·Δt),min(v now +a·Δt,v max )]
ω∈[max(-ω max ,ω now -β·Δt),min(ω now +β·Δt,ω max )]。
7. the indoor mobile robot precise positioning navigation algorithm with smooth track gradient according to claim 6, wherein the indoor mobile robot precise positioning navigation algorithm is characterized in that: the algorithm of the local planning comprises the following steps:
(10) The linear speed and angular speed range of the robot are respectively discretized into M values and N values, wherein M and N are manually preset values, and then are arranged and combined into M multiplied by N (linear speed and angular speed) value combinations;
(11) For each combination of M×N (v, ω), the robot trajectory is calculated by simulation while keeping unchanged in Δt time, and the robot trajectory is a circular arc, and the current pose of the robot is recorded as (x) now ,y now ,θ now ) The trajectory of the robot after Δt time is:
θ t ＝θ now +ωΔt
(12) Calculating and simulating the track of the robot and the nearest distance d of the obstacle, judging whether collision occurs with the obstacle in the map, and eliminating the combination of collision to obtain a residual track set T;
(13) For the tracks in the set T, firstly, carrying out normalization calculation on the speeds (v, omega), the nearest distance d to the obstacle and the attitude angle theta in the set, and mapping the values of the speeds (v, omega), the nearest distance d to the obstacle and the attitude angle theta to be between [0,1 ];
calculating the speed of the minimum track of the cost in the T according to a series of costs, and taking the speed as a final control instruction of the robot;
the cost is defined as follows:
TotalCost＝GoalCost+ObstacleCost+VelocityCost
GoalCost＝distance(Goal,RobotPos)
ObstacleCost＝-min_distanace_to_obstacle
VelocityCost＝α·v+β·ω
the Goalcost is the Euclidean distance from the current position RobotPos of the robot to the target point Goal, the obstaclecast is used for measuring the distance from the robot to the obstacle, and the VelocityCost is a punishment item for the linear speed and the angular speed.
It is noted that 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. Moreover, 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.
Finally, it should be noted that: the foregoing description is only a preferred embodiment of the present invention, and the present invention is not limited thereto, but it is to be understood that modifications and equivalents of some of the technical features described in the foregoing embodiments may be made by those skilled in the art, although the present invention has been described in detail with reference to the foregoing embodiments. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention should be included in the protection scope of the present invention.
Claims (8)
1. An indoor mobile robot accurate positioning navigation algorithm with smooth track gradient is characterized in that: the navigation algorithm adopts a navigation system, the system comprises a map maintenance module, a global planner module, a positioning module and a local planner module, wherein the sub-algorithm of the map maintenance module is used for carrying out online maintenance and updating on a map based on a grid map constructed by a pre-used positioning and mapping technology and real-time sensing information of a laser radar sensor, the global planner module is used for planning an obstacle-free path from a starting point to a terminal point for a robot based on the latest map information provided by the map maintenance module, the positioning module is used for calculating the gesture of the robot in a map coordinate system by utilizing environment data observed by the laser radar sensor based on the map information, the gesture comprises position coordinates and orientation angles, and the local planner module is used for following the path from the global planner and providing pose information for feedback by utilizing the positioning module.
2. The precise positioning navigation algorithm of the indoor mobile robot with smooth track gradient according to claim 1, wherein the precise positioning navigation algorithm is characterized in that: the map maintenance module part is used for carrying out maintenance on a grid map which is built by a user in advance by using a synchronous positioning and mapping technology and is divided into a global map and a local map, wherein the grid map is used for dividing a plane map into a plurality of small grids by a square with a certain resolution, namely the map is discretized into a two-dimensional matrix, each small grid is called a grid, the states of the grids are represented by different grid values, the values represent the probability that the grid is an obstacle, the probability is 0 and represents a passable area as shown in a white grid of the graph, the probability is 1 and represents an obstacle as shown in a black grid of the graph, and the probability is between 0 and 1 and represents a robot unexplored area;
based on the original grid map, the algorithm represents the global map and the local map in the form of a cost map, namely, the cost is generally represented by an integer between 0 and 255, in the algorithm, the distance between the barrier of the grid and the geometric center of the robot is represented by a value between 128 and 255, the greater the probability of collision is represented by greater, and the distance between the grid and the outermost contour of the robot is represented by 0 to 128;
the global map maintained by the algorithm is a cost map of the same size as the original grid map, while the local map is a square area with a size centered on the robot, the resolution of the local map is higher than that of the global map, and when a sensor such as a laser radar installed on the robot finds a new obstacle in the environment, the information is updated to the grid map first, and then to the global map and the local map.
3. The indoor mobile robot accurate positioning navigation algorithm with smooth track gradient according to claim 2, wherein the indoor mobile robot accurate positioning navigation algorithm is characterized in that: the algorithm of the grid map update is as follows, for one grid in the map, the probability that the grid is an obstacle is represented by P (state=1), the probability that the grid is a passable area is represented by P (state=0), and the two probabilities satisfy:
P(state＝1)+P(state＝0)＝1
the ratio of these two probabilities is noted as:
in addition, the current observation data of the laser radar sensor on the environment is z, and the probability that the grid is an obstacle and a passable area after z is given is respectively as follows:
the ratio of the two is:
the two sides take the logarithm:
of which onlyThe term contains the measured value, this log ratio is called the measurement model, and this value is related to the actual measurement situation, depending on whether the rays of the lidar hit the current grid, the initial value of log Q (state) is:
4. the indoor mobile robot precise positioning navigation algorithm with smooth track gradient according to claim 3, wherein the indoor mobile robot precise positioning navigation algorithm is characterized in that: the global planner module can plan a smooth global path based on a Dijkstra path planning algorithm of gradient descent, the Dijkstra algorithm can find the shortest path of a grid from a starting point grid to an end point, and in the planning process of the algorithm, a step of calculating the potential energy value of a grid is added in the process of finding the shortest path by the Dijkstra algorithm, namely, the grid near the shortest path updates a potential energy value, then starts from the grid at the end point, calculates potential energy gradients in 8 directions according to the potential energy values of 8 neighbor grids near the grid, acquires the next path point along the direction with the maximum potential energy gradient according to a set step length, then starts from the next path point, continues to calculate the next path point along the direction with the maximum potential energy gradient until reaching the vicinity of the starting point, and the planned path is a smooth curve because the distance from the next path point to the potential energy gradient is not a fixed grid length distance, wherein the quadric interpolation function is a current grid with the potential energy value, and the quadric interpolation function is based on the current grid value of the left and the right lattice interpolation function.
5. The precise positioning navigation algorithm of the indoor mobile robot with smooth track gradient of claim 4, wherein the precise positioning navigation algorithm is characterized in that: the positioning module performs positioning by using a particle filtering algorithm based on a Bayesian filtering framework based on the occupancy grid map and the current laser radar data,
the positioning process comprises the following main steps:
(1) Randomly generating N particles, wherein each particle represents a probability hypothesis for the pose of the robot in the map;
(2) Calculating the weight of each particle according to the latest observed laser radar data, wherein the weight of the position of the particle is larger in accordance with the observed data, and otherwise, the weight is smaller;
(3) And (3) forming a new set by the particles with the weight values, resampling according to the weight values to obtain a new particle set, wherein the weight is the largest in the new particle set and is used for representing the current pose of the robot.
6. Indoor mobile machine with smooth track gradient as set forth in claim 5The accurate positioning navigation algorithm for people is characterized in that: the local planner module is based on the solution of the loss function of the robot hard constraint, and the maximum acceleration of the robot is set as a, the maximum angular acceleration is set as beta, the simulation time of each planning is set as deltat, and the current linear speed of the robot is set as v now The current angular velocity is ω now For the robot, after Δt time, the range of values of the linear velocity and the angular velocity is:
v∈[v now -a·Δt，v now +a·Δt]
ω∈[ω now -β·Δt，ω now +β·Δt]
in addition, the values considering the theoretical maximum linear velocity and angular velocity of the robot are: v max And omega max In the safety consideration, and the robot is not allowed to have backward, namely v is more than or equal to 0, the range of values of the linear speed and the angular speed of the robot is as follows:
v∈[max(0，v now -a·Δt)，min(v now +a·Δt，v max )]
ω∈[max(-ω max ，ω now -β·Δt)，min(ω now +β·Δt，ω max )]。
7. the precise positioning navigation algorithm of the indoor mobile robot with smooth track gradient of claim 6, wherein the precise positioning navigation algorithm is characterized in that: the algorithm of the local planning comprises the following steps:
(1) The linear speed and angular speed range of the robot are respectively discretized into M values and N values, wherein M and N are manually preset values, and then are arranged and combined into M multiplied by N (linear speed and angular speed) value combinations;
(2) For each combination of M×N (v, ω), the robot trajectory is calculated by simulation while keeping unchanged in Δt time, and the robot trajectory is a circular arc, and the current pose of the robot is recorded as (x) now ，y now ，θ now ) The trajectory of the robot after Δt time is:
θ t ＝θ now +ωΔt
(3) Calculating and simulating the track of the robot and the nearest distance d of the obstacle, judging whether collision occurs with the obstacle in the map, and eliminating the combination of collision to obtain a residual track set T;
(4) For the tracks in the set T, firstly, carrying out normalization calculation on the speeds (v, omega), the nearest distance d to the obstacle and the attitude angle theta in the set, and mapping the values of the speeds (v, omega), the nearest distance d to the obstacle and the attitude angle theta to be between [0,1 ];
(5) The speed of the minimum track of the cost in the T is calculated according to a series of costs and is taken as the final control instruction of the robot.
8. The precise positioning navigation algorithm of the indoor mobile robot with smooth track gradient of claim 7, wherein the precise positioning navigation algorithm is characterized in that: the cost is defined as follows:
TotalCost＝GoalCost+ObstacleCost+VelocityCost
GoalCost＝distance(Goal，RobotPos)
ObstacleCost＝-min_distanace_to_obstacle
VelocityCost＝α·v+β·ω
the Goalcost is the Euclidean distance from the current position RobotPos of the robot to the target point Goal, the obstaclecast is used for measuring the distance from the robot to the obstacle, and the VelocityCost is a punishment item for the linear speed and the angular speed.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310491190.0A CN116592887A (en) | 2023-05-05 | 2023-05-05 | Accurate positioning navigation algorithm of indoor mobile robot with smooth track gradient |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310491190.0A CN116592887A (en) | 2023-05-05 | 2023-05-05 | Accurate positioning navigation algorithm of indoor mobile robot with smooth track gradient |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN116592887A true CN116592887A (en) | 2023-08-15 |
Family
ID=87600003
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310491190.0A Pending CN116592887A (en) | 2023-05-05 | 2023-05-05 | Accurate positioning navigation algorithm of indoor mobile robot with smooth track gradient |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116592887A (en) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117330081A (en) * | 2023-11-08 | 2024-01-02 | 广东拓普视科技有限公司 | Perception navigation device and method based on robot |
-
2023
- 2023-05-05 CN CN202310491190.0A patent/CN116592887A/en active Pending
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117330081A (en) * | 2023-11-08 | 2024-01-02 | 广东拓普视科技有限公司 | Perception navigation device and method based on robot |
| CN117330081B (en) * | 2023-11-08 | 2024-05-10 | 广东拓普视科技有限公司 | Perception navigation device and method based on robot |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111240319B (en) | Outdoor multi-robot cooperative operation system and method thereof | |
| CN112068588A (en) | Unmanned aerial vehicle trajectory generation method based on flight corridor and Bezier curve | |
| CN105865449B (en) | Hybrid positioning method of mobile robot based on laser and vision | |
| CN110262495A (en) | Mobile robot autonomous navigation and pinpoint control system and method can be achieved | |
| CN102759357B (en) | Cooperative real-time path planning method for multiple unmanned aerial vehicles (UAVs) in case of communication latency | |
| KR101372482B1 (en) | Method and apparatus of path planning for a mobile robot | |
| CN104062902B (en) | Delta robot time optimal trajectory planning method | |
| CN108205318A (en) | Method for planning track of robot and device | |
| CN109520507A (en) | A kind of unmanned plane real-time route planing method based on improvement RRT | |
| CN116592887A (en) | Accurate positioning navigation algorithm of indoor mobile robot with smooth track gradient | |
| JP5068840B2 (en) | Robot program and information processing apparatus program | |
| CN109557929B (en) | The motion control method and device of mobile robot | |
| CN110632932A (en) | Local path planning algorithm based on membrane calculation and particle swarm optimization | |
| Geng et al. | UAV surveillance mission planning with gimbaled sensors | |
| CN114967744A (en) | Planning method for multi-unmanned aerial vehicle cooperative obstacle avoidance | |
| CN114527753A (en) | Man-machine integrated building path planning method, computer device and program product | |
| CN108363395A (en) | A kind of method of AGV automatic obstacle avoidings | |
| Liao et al. | Deploy indoor 2D laser SLAM on a raspberry pi-based mobile robot | |
| CN115268448A (en) | Multi-robot path planning method based on conflict search and speed obstacle | |
| CN114755373A (en) | Air pollution source early warning positioning method based on multi-robot formation | |
| CN115388892A (en) | Multisensor fusion SLAM method based on improved RBPF-SLAM algorithm | |
| Hu et al. | Optimal path planning for mobile manipulator based on manipulability and localizability | |
| CN116380039A (en) | Mobile robot navigation system based on solid-state laser radar and point cloud map | |
| Zhang et al. | Research on agv navigation simulation in printing wisdom factory | |
| CN114740849B (en) | Mobile robot autonomous navigation method and device based on pedestrian walking decision rule |
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 |