CN108375976A - A kind of service robot navigation methods and systems - Google Patents
A kind of service robot navigation methods and systems Download PDFInfo
- Publication number
- CN108375976A CN108375976A CN201810058588.4A CN201810058588A CN108375976A CN 108375976 A CN108375976 A CN 108375976A CN 201810058588 A CN201810058588 A CN 201810058588A CN 108375976 A CN108375976 A CN 108375976A
- Authority
- CN
- China
- Prior art keywords
- map
- sub
- odds
- service robot
- pose
- 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 61
- 230000008569 process Effects 0.000 claims abstract description 26
- 238000001514 detection method Methods 0.000 claims description 23
- 238000011156 evaluation Methods 0.000 claims description 9
- 238000005070 sampling Methods 0.000 claims description 3
- 230000008030 elimination Effects 0.000 claims description 2
- 238000003379 elimination reaction Methods 0.000 claims description 2
- 238000000059 patterning Methods 0.000 abstract description 8
- 238000012360 testing method Methods 0.000 abstract description 2
- 238000004804 winding Methods 0.000 description 11
- 238000005457 optimization Methods 0.000 description 6
- 238000009825 accumulation Methods 0.000 description 4
- 230000004888 barrier function Effects 0.000 description 4
- 238000005516 engineering process Methods 0.000 description 4
- 230000008901 benefit Effects 0.000 description 3
- 230000006870 function Effects 0.000 description 3
- 239000002245 particle Substances 0.000 description 3
- 241001269238 Data Species 0.000 description 2
- 230000010391 action planning Effects 0.000 description 2
- 230000008859 change Effects 0.000 description 2
- 238000013500 data storage Methods 0.000 description 2
- 230000008520 organization Effects 0.000 description 2
- 238000012797 qualification Methods 0.000 description 2
- 230000003068 static effect Effects 0.000 description 2
- 238000010408 sweeping Methods 0.000 description 2
- 238000012952 Resampling Methods 0.000 description 1
- 230000001154 acute effect Effects 0.000 description 1
- 230000003044 adaptive effect Effects 0.000 description 1
- 238000007792 addition Methods 0.000 description 1
- 230000005540 biological transmission Effects 0.000 description 1
- 238000006243 chemical reaction Methods 0.000 description 1
- 238000004140 cleaning Methods 0.000 description 1
- 238000010276 construction Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000007257 malfunction Effects 0.000 description 1
- 238000007726 management method Methods 0.000 description 1
- 239000011159 matrix material Substances 0.000 description 1
- 230000007246 mechanism Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000012544 monitoring process Methods 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
- G05D1/0238—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using obstacle or wall sensors
- G05D1/024—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using obstacle or wall sensors in combination with a laser
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0214—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory in accordance with safety or protection criteria, e.g. avoiding hazardous areas
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0221—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0223—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving speed control of the vehicle
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0276—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle
Abstract
The invention discloses a kind of service robot navigation methods and systems, to solve the problems, such as that existing airmanship does not have loopback test positioning to will produce error.This method includes：The accumulated error eliminated map structuring and generated in the process is detected by closed loop using cartographer algorithms；Global path planning is carried out to the map of structure using random tree algorithm is quickly traversed；Local paths planning is carried out to the map of structure using the dynamic window algorithm of local avoidance.The present invention is based on cartographer algorithms to detect the error eliminated in patterning process by closed loop, keeps positioning more accurate, reduces error.
Description
Technical field
The present invention relates to location and navigation technology field more particularly to a kind of service robot navigation methods and systems.
Background technology
Service robot is a younger members in family of robot, and up to the present still neither one is stringent determines
Justice.Country variant is different to the understanding of service robot.
Professional domain service robot and individual/home-services robot, the application range of service robot can be divided into
It is very wide, mainly it is engaged in the work such as maintaining, repairing, transport, cleaning, security personnel, rescue, monitoring.
ROS is robot software's platform, and the function of similar operations system can be provided for disparate computers cluster.Origin
It is a kind of distributed processing frame, a kind of robot back operation system increased income in 2007.For example, hardware abstraction description,
The transmission etc. of message between bsp driver management, program.
Gmapping location algorithms mainly apply the RBPF methods in particle filter, and the process of scan matching is to estimate first
Robot pose is counted, the method declined using gradient, using the map, laser point and the robot pose that currently build as initially estimating
Evaluation, in order to solve the particle dissipation issues in PF, it is proposed that the mechanism of adaptive resampling can be reduced to a certain extent
For the particle number for ensureing needed for positioning accuracy.It but has the disadvantage that dependence odometer, unmanned plane can not be applicable in and ground is uneven
Situation, and since PF follows Markov it is assumed that current state is only related with previous moment, so winding can not be carried out.
Hector location algorithms mainly match laser point with existing map, utilize the map pair obtained
Laser beam dot matrix optimizes, the probability of expression and occupancy grid of the estimation laser point in map.It is solved using Gauss-Newton
The problem of scan-match (scan-map), finds the rigid body conversion that laser point set is mapped to existing map；To avoid part most
It is small rather than complete acute minimum, use multiresolution map.Disadvantage is the amendment of not map, once map malfunctions, later
With can go wrong, the same algorithm does not have winding detection yet.
There is no winding detection to will produce error accumulation, error accumulation cannot be eliminated in time, then can lead to position inaccurate
Problem.
Invention content
The technical problem to be solved in the present invention is designed to provide a kind of service robot navigation methods and systems, to solve
Certainly existing airmanship does not have the problem of loopback test positioning will produce error.
To achieve the goals above, the technical solution adopted by the present invention is：
A kind of service robot air navigation aid, including step：
The accumulated error eliminated map structuring and generated in the process is detected by closed loop using cartographer algorithms；
Global path planning is carried out to the map of structure using random tree algorithm is quickly traversed；
Local paths planning is carried out to the map of structure using the dynamic window algorithm of local avoidance.
Further, described detected by closed loop using cartographer algorithms eliminates what map structuring generated in the process
The step of accumulated error, specifically includes：
It establishes the sub- map of closed loop detection and assigns the probability value of the mesh point of the probabilistic grid of the sub- map；
Using Ceres-based matching scanning information points relative to current sub- map pose to find in the sub- map
Maximum probability scanning information point set pose；
Optimize the pose of all scanning information points and sub- map using SPA and stores the relative pose；
Branch-and-bound and DFS search strategies is used to carry out closed loop detection and the solution of relative pose to eliminate accumulated error.
Further, the method for the probability value of the mesh point of the probabilistic grid for assigning the sub- map specifically includes：
Judge the mesh point whether assignment, if it is not, according to the mesh point whether hit assign hit probability value or
Miss probability value；
If mesh point assignment, according to following formula update probability value：
Mnew(x)=clamp (odds-1(odds(Mold(x))·odds(phits)))；
Mnew(x)=clamp (odds-1(odds(Mold(x))·odds(pmiss)))。
Further, described using quickly traversing the step of random tree algorithm carries out global path planning to the map of structure
It specifically includes：
Random tree is guided to be given birth to towards target direction by quickly traversing random tree algorithm in the introducing of Artificial Potential Field Method path planning
It is long；The formula that the introducing quickly traverses random tree algorithm is as follows：
Further, the dynamic window algorithm using local avoidance carries out local paths planning to the map of structure
Step specifically includes：
The current state of service robot is sampled；
The travel route after the service robot preset time is calculated for each sample rate；
It is scored a plurality of travel route using default evaluation criterion；
Path is selected according to appraisal result.
A kind of service robot navigation system, including：
Error concealment module, for being produced during detecting elimination map structuring by closed loop using cartographer algorithms
Raw accumulated error；
Global path planning module, for carrying out global path rule to the map of structure using quickly traversing random tree algorithm
It draws；
Local paths planning module carries out local road for the dynamic window algorithm using local avoidance to the map of structure
Diameter is planned.
Further, the error concealment module specifically includes：
Sub- map establishes unit, the net for establishing the sub- map that closed loop detects and the probabilistic grid for assigning the sub- map
The probability value of lattice point；
Local matching scanning element, for matching scanning information point relative to current sub- map using Ceres-based
Pose is to find the pose in the scanning information point set of the maximum probability of the sub- map；
Global closed loop optimizes unit, and the pose for optimizing all scanning information points and sub- map using SPA simultaneously will be described
Relative pose is stored；
Accumulated error eliminates unit, for carrying out closed loop detection and relative pose using branch-and-bound and DFS search strategies
Solution to eliminate accumulated error.
Further, whether sub- map establishes unit and is specifically used for judging mesh point assignment, if it is not, according to described
Whether mesh point, which hits, assigns hit probability value or miss probability value；If mesh point assignment, according to following formula
Update probability value：
Mnew(x)=clamp (odds-1(odds(Mold(x))·odds(phits)))；
Mnew(x)=clamp (odds-1(odds(Mold(x))·odds(pmiss)))。
Further, the global path planning module specifically includes：
Unit is introduced, for guiding random tree by quickly traversing random tree algorithm in the introducing of Artificial Potential Field Method path planning
It is grown towards target direction；The formula that the introducing quickly traverses random tree algorithm is as follows：
Further, the local paths planning module specifically includes：
Sampling unit is sampled for the current state to service robot；
Computing unit, for calculating the travel route after the service robot preset time for each sample rate；
Score unit, for being scored a plurality of travel route using default evaluation criterion；
Selecting unit, for selecting path according to appraisal result.
The present invention has the following advantages compared with traditional technology：
The present invention is based on cartographer algorithms to detect the error eliminated in patterning process by closed loop, makes positioning more
Accurately, reduce error.
Description of the drawings
Fig. 1 is a kind of service robot air navigation aid flow chart that embodiment one provides；
Fig. 2 is a kind of service robot navigation system structure chart that embodiment two provides.
Specific implementation mode
Following is a specific embodiment of the present invention in conjunction with the accompanying drawings, technical scheme of the present invention will be further described,
However, the present invention is not limited to these examples.
Embodiment one
A kind of service robot air navigation aid is present embodiments provided, as shown in Figure 1, including step：
S11：The accumulated error eliminated and generated in map patterning process is detected by closed loop using cartographer algorithms；
S12：Global path planning is carried out to the map of structure using random tree algorithm is quickly traversed；
S13：Local paths planning is carried out to the map of structure using the dynamic window algorithm of local avoidance.
The cartographer algorithms of the present embodiment be by closed loop detection come eliminate generated in map patterning process it is tired
Product error.Basic unit for closed loop detection is sub- map.One sub- map is made of a certain number of laser scannings, will
When its corresponding sub- map is inserted into one laser scanning, the existing laser scanning of sub- map can be based on and other sensing datas are estimated
Count its optimum position in sub- map.Error of the sub- map within the section time is sufficiently small, however as the passage of time, error
Accumulation can be increasing.
In the present embodiment, step S11 is to detect to eliminate map patterning process by closed loop using cartographer algorithms
The accumulated error of middle generation.
Accumulated error is eliminated by closed loop detection suitably optimizes the pose of sub- map, it is excellent to convert problem to pose
The problem of change.Pose optimization problem solves nonlinear least square problem.As soon as when the structure of sub- map is completed,
It is when new laser scanning there will be no to be inserted into the sub- map, which can be added in closed loop detection.Closed loop detects nationwide examination for graduation qualification
Consider all sub- maps that establishment is completed.When a new laser scanning is added in map, if the laser scanning
The pose of some laser scanning of the sub- map of certain in pose and map is estimated relatively, then being matched by corresponding laser
Strategy will find the closed loop.The laser matching strategy near the estimation pose of laser scanning that map is newly added by taking one
A window, and then a possible matching of the laser scanning is found in the window, if having found good enough
Match, then the matched closed loop constraint can be added in pose optimization problem.
In the present embodiment, step S11 is specifically included：
It establishes the sub- map of closed loop detection and assigns the probability value of the mesh point of the probability net of sub- map；
Using Ceres-based matching scanning information points relative to current sub- map pose to find the probability of sub- map
The pose of maximum scanning information point set；
Optimize the pose of all scanning information points and sub- map using SPA and stores relative pose；
Branch-and-bound and DFS search strategies is used to carry out closed loop detection and the solution of relative pose to eliminate accumulated error.
Wherein, the step of probability value for assigning the mesh point of the probabilistic grid of sub- map, specifically includes：
Judge the mesh point whether assignment, if it is not, according to the mesh point whether hit assign hit probability value or
Miss probability value；
If mesh point assignment, according to following formula update probability value：
Mnew(x)=clamp (odds-1(odds(Mold(x))·odds(phits)))；
Mnew(x)=clamp (odds-1(odds(Mold(x))·odds(pmiss)))。
Specifically, algorithm flow can be divided into following steps：
A. sub- map is established：Point set is scanned whenever one to be inserted into this probabilistic grid, a life for including mesh point
Middle set and a miss set can be all recalculated.For hitting each time, this nearest mesh point is added
Hitting set is inserted into each picture intersected with a line between sweep starting point and each scanning element for miss each time
The associated mesh point of element, the mesh point in hitting set need not be inserted into.For being not observed before each
Mesh point all according to set where them can be hitting set or miss set assign a probability value phit or
Pmiss, if a mesh point x has been observed to, it will update this probability value by mode below：
Mnew(x)=clamp (odds-1(odds(Mold(x))·odds(phits)))；
Mnew(x)=clamp (odds-1(odds(Mold(x))·odds(pmiss)))。
B. partial sweep matches：Before by a scanning information point set intron map, this scanning information point set
Pose optimized relative to the current sub- map in part using the scan matching method of Ceres-based.Scan matching mistake
The purpose of journey is the pose of the scanning information point set in order to find a maximum probability in sub- map, using this process as
Non-linear least square problem.
C. global closed loop optimizes：Cartographer handles larger space by creating many small sub- maps.By
In scanning information and several sub- map match for managing its nearest scanning information, so partial sweep matching process can be slowly
Accumulated error.Optimize used here as SPA.It is every other by sweeping when group map no longer changes other than these relative poses
Retouch information it is corresponding with its from gambler form to being all artificially closed loop.The rear ends SLAM run scan matching process, when looking for
When to a good scan matching, corresponding relative pose would be added in optimization problem.
D. accumulated error is eliminated：The detection of winding is carried out using branch-and-bound and the DFS strategy searched for and relative pose is asked
Solution, to eliminate accumulated error.Compared to traditional first detection winding, solving for relative pose, it appears it is more unified in structure,
Convert the process for establishing winding to a search procedure.And on the other hand, when completion is to the tree-like structure of discrete candidate solution space
After making, in tree quickly to the search procedure speed of solution, the remaining selection for the node during achievement is by antithetical phrase
Completing with calculating for figure, uses the intermediate structure as sub- map winding process can be done in real time, to by returning
Ring constantly adjusts sub- map to eliminate accumulated error.
In the present embodiment, step S12 is to utilize the map progress global path rule for quickly traversing random tree algorithm to structure
It draws.
Wherein, step S12 is specifically included：
Random tree is guided to be given birth to towards target direction by quickly traversing random tree algorithm in the introducing of Artificial Potential Field Method path planning
It is long；The formula that the introducing quickly traverses random tree algorithm is as follows：
Quickly traversal random tree RRT is a kind of tree data storage organization and algorithm, is established by incremental method, and fast
Speed reduces distance of the random selection point with tree, for effectively searching for non-convex high-dimensional space, especially suitable for including barrier
Hinder the action planning under the conditions of object and nonholonomic system or IK Solvers differential constraint.
The characteristics of RRT, is that algorithm construction is simple, and can quickly traverse space does not explore region, and by setting manually
Surely it finds and realizes that the function of this effect is to be difficult.RRT can add the search in space on the basis of rudimentary algorithm simultaneously
Enter to carry the control function of input parameter, algorithm is made to can be adapted for the route searching under various boundary conditions.
Artificial Potential Field Method path planning is a kind of virtual force method proposed by Khatib.Its basic thought is by robot
Movement in ambient enviroment, the movement being designed in a kind of abstract artificial gravitational field, target point generate mobile robot
" gravitation ", barrier generate " repulsion " to mobile robot, and the movement of mobile robot is controlled finally by resultant force is sought.
The present embodiment uses improved Rapid-Exploring Random Tree Algorithm, specifically includes：
A* algorithms are to solve the most effective direct search method of shortest path in a kind of static road network；
D* algorithms are dynamic A* algorithms.
In order to solve stochastic problems, the thought of gravitation in Artificial Potential Field is introduced into RRT algorithms, guides random tree towards mesh
Direction growth is marked, without being modeled to global context, reduces planning time, is represented by：
It is usually smoother and safe to cook up the path come using potential field method, but this method has part most
Advantage problem.
In the present embodiment, step S13 is to carry out local road to the map of structure using the dynamic window algorithm of local avoidance
Diameter is planned.
Wherein, step S13 is specifically included：
The current state of service robot is sampled；
The travel route after service robot preset time is calculated for each sample rate；
It is scored a plurality of travel route using default evaluation criterion；
Path is selected according to appraisal result.
Specifically, dynamic window DWA algorithm of the local paths planning using local avoidance.First service robot is worked as
Preceding state is sampled, and for each sample rate, is calculated service robot and is travelled the state after a period of time with present speed,
Obtain a travel route.It recycles evaluation criterion to give a mark a plurality of route, according to marking, selects score highest optimal
Path.Continue to repeat the above process, carries out next local paths planning.The shortcomings that compensating for global path planning in this way.Make
Path planning more science is comprehensive.
The present embodiment is accurately positioned and is navigated to server machine people using cartographer algorithms, is avoided tired
The problem of product error and local best points, keep positioning more accurate.
Embodiment two
A kind of service robot navigation system is present embodiments provided, as shown in Fig. 2, including：
Error concealment module 21 is eliminated for being detected by closed loop using cartographer algorithms in map patterning process
The accumulated error of generation；
Global path planning module 22, for carrying out global path to the map of structure using quickly traversing random tree algorithm
Planning；
Local paths planning module 23 carries out part for the dynamic window algorithm using local avoidance to the map of structure
Path planning.
The cartographer algorithms of the present embodiment be by closed loop detection come eliminate generated in map patterning process it is tired
Product error.Basic unit for closed loop detection is sub- map.One sub- map is made of a certain number of laser scannings, will
When its corresponding sub- map is inserted into one laser scanning, the existing laser scanning of sub- map can be based on and other sensing datas are estimated
Count its optimum position in sub- map.Error of the sub- map within the section time is sufficiently small, however as the passage of time, error
Accumulation can be increasing.
In the present embodiment, error concealment module 21, which is used to detect by closed loop using cartographer algorithms, eliminates map
The accumulated error generated in patterning process.
Accumulated error is eliminated by closed loop detection suitably optimizes the pose of sub- map, it is excellent to convert problem to pose
The problem of change.Pose optimization problem solves nonlinear least square problem.As soon as when the structure of sub- map is completed,
It is when new laser scanning there will be no to be inserted into the sub- map, which can be added in closed loop detection.Closed loop detects nationwide examination for graduation qualification
Consider all sub- maps that establishment is completed.When a new laser scanning is added in map, if the laser scanning
The pose of some laser scanning of the sub- map of certain in pose and map is estimated relatively, then being matched by corresponding laser
Strategy will find the closed loop.The laser matching strategy near the estimation pose of laser scanning that map is newly added by taking one
A window, and then a possible matching of the laser scanning is found in the window, if having found good enough
Match, then the matched closed loop constraint can be added in pose optimization problem.
In the present embodiment, error concealment module 21 specifically includes：
Sub- map establishes unit, the mesh point for establishing the sub- map that closed loop detects and the probability net for assigning sub- map
Probability value；
Local matching scanning element, for matching scanning information point relative to current sub- map using Ceres-based
Pose is to find the pose of the scanning information point set of the maximum probability of sub- map；
Global closed loop optimizes unit, and the pose for optimizing all scanning information points and sub- map using SPA simultaneously will be opposite
Pose is stored；
Accumulated error eliminates unit, for carrying out closed loop detection and relative pose using branch-and-bound and DFS search strategies
Solution to eliminate accumulated error.
Wherein, it charges into certainly and establishes unit specifically for judging the mesh point whether assignment, if it is not, according to the grid
Whether point, which hits, assigns hit probability value or miss probability value；
If mesh point assignment, according to following formula update probability value：
Mnew(x)=clamp (odds-1(odds(Mold(x))·odds(phits)))；
Mnew(x)=clamp (odds-1(odds(Mold(x))·odds(pmiss)))。
Specifically, algorithm flow can be divided into following steps：
A. sub- map is established：Point set is scanned whenever one to be inserted into this probabilistic grid, a life for including mesh point
Middle set and a miss set can be all recalculated.For hitting each time, this nearest mesh point is added
Hitting set is inserted into each picture intersected with a line between sweep starting point and each scanning element for miss each time
The associated mesh point of element, the mesh point in hitting set need not be inserted into.For being not observed before each
Mesh point all according to set where them can be hitting set or miss set assign a probability value phit or
Pmiss, if a mesh point x has been observed to, it will update this probability value by mode below：
Mnew(x)=clamp (odds-1(odds(Mold(x))·odds(phits)))；
Mnew(x)=clamp (odds-1(odds(Mold(x))·odds(pmiss)))。
B. partial sweep matches：Before by a scanning information point set intron map, this scanning information point set
Pose optimized relative to the current sub- map in part using the scan matching method of Ceres-based.Scan matching mistake
The purpose of journey is the pose of the scanning information point set in order to find a maximum probability in sub- map, using this process as
Non-linear least square problem.
C. global closed loop optimizes：Cartographer handles larger space by creating many small sub- maps.By
In scanning information and several sub- map match for managing its nearest scanning information, so partial sweep matching process can be slowly
Accumulated error.Optimize used here as SPA.It is every other by sweeping when group map no longer changes other than these relative poses
Retouch information it is corresponding with its from gambler form to being all artificially closed loop.The rear ends SLAM run scan matching process, when looking for
When to a good scan matching, corresponding relative pose would be added in optimization problem.
D. accumulated error is eliminated：The detection of winding is carried out using branch-and-bound and the DFS strategy searched for and relative pose is asked
Solution, to eliminate accumulated error.Compared to traditional first detection winding, solving for relative pose, it appears it is more unified in structure,
Convert the process for establishing winding to a search procedure.And on the other hand, when completion is to the tree-like structure of discrete candidate solution space
After making, in tree quickly to the search procedure speed of solution, the remaining selection for the node during achievement is by antithetical phrase
Completing with calculating for figure, uses the intermediate structure as sub- map winding process can be done in real time, to by returning
Ring constantly adjusts sub- map to eliminate accumulated error.
In the present embodiment, global path planning module 22 be used for using quickly traverse random tree algorithm to the map of structure into
Row global path planning.
Wherein, global path planning module 22 specifically includes：
Unit is introduced, for guiding random tree by quickly traversing random tree algorithm in the introducing of Artificial Potential Field Method path planning
It is grown towards target direction；The formula that the introducing quickly traverses random tree algorithm is as follows：
Quickly traversal random tree RRT is a kind of tree data storage organization and algorithm, is established by incremental method, and fast
Speed reduces distance of the random selection point with tree, for effectively searching for non-convex high-dimensional space, especially suitable for including barrier
Hinder the action planning under the conditions of object and nonholonomic system or IK Solvers differential constraint.
Artificial Potential Field Method path planning is a kind of virtual force method proposed by Khatib.Its basic thought is by robot
Movement in ambient enviroment, the movement being designed in a kind of abstract artificial gravitational field, target point generate mobile robot
" gravitation ", barrier generate " repulsion " to mobile robot, and the movement of mobile robot is controlled finally by resultant force is sought.
The present embodiment uses improved Rapid-Exploring Random Tree Algorithm, specifically includes：
A* algorithms are to solve the most effective direct search method of shortest path in a kind of static road network；
D* algorithms are dynamic A* algorithms.
In order to solve stochastic problems, the thought of gravitation in Artificial Potential Field is introduced into RRT algorithms, guides random tree towards mesh
Direction growth is marked, without being modeled to global context, reduces planning time, is represented by：
It is usually smoother and safe to cook up the path come using potential field method, but this method has part most
Advantage problem.
In the present embodiment, local paths planning module 23 is used to utilize the dynamic window algorithm of local avoidance to the ground of structure
Figure carries out local paths planning.
Wherein, local paths planning module 23 specifically includes：
Sampling unit is sampled for the current state to service robot；
Computing unit, for for the travel route after each sample rate calculating service robot preset time；
Score unit, for being scored a plurality of travel route using default evaluation criterion；
Selecting unit, for selecting path according to appraisal result.
Specifically, dynamic window DWA algorithm of the local paths planning using local avoidance.First service robot is worked as
Preceding state is sampled, and for each sample rate, is calculated service robot and is travelled the state after a period of time with present speed,
Obtain a travel route.It recycles evaluation criterion to give a mark a plurality of route, according to marking, selects score highest optimal
Path.Continue to repeat the above process, carries out next local paths planning.The shortcomings that compensating for global path planning in this way.Make
Path planning more science is comprehensive.
The present embodiment is accurately positioned and is navigated to server machine people using cartographer algorithms, is avoided tired
The problem of product error and local best points, keep positioning more accurate.
Specific embodiment described herein is only an example for the spirit of the invention.Technology belonging to the present invention is led
The technical staff in domain can make various modifications or additions to the described embodiments or replace by a similar method
In generation, however, it does not deviate from the spirit of the invention or beyond the scope of the appended claims.
Claims (10)
1. a kind of service robot air navigation aid, which is characterized in that including step：
The accumulated error eliminated map structuring and generated in the process is detected by closed loop using cartographer algorithms；
Global path planning is carried out to the map of structure using random tree algorithm is quickly traversed；
Local paths planning is carried out to the map of structure using the dynamic window algorithm of local avoidance.
2. a kind of service robot air navigation aid according to claim 1, which is characterized in that the utilization
The step of cartographer algorithms detect the accumulated error generated during elimination map structuring by closed loop specifically includes：
It establishes the sub- map of closed loop detection and assigns the probability value of the mesh point of the probabilistic grid of the sub- map；
Using Ceres-based matching scanning information points relative to current sub- map pose to find in the general of the sub- map
The pose of the maximum scanning information point set of rate；
Optimize the pose of all scanning information points and sub- map using SPA and stores the relative pose；
Branch-and-bound and DFS search strategies is used to carry out closed loop detection and the solution of relative pose to eliminate accumulated error.
3. a kind of service robot air navigation aid according to claim 2, which is characterized in that described to assign the sub- map
The method of probability value of mesh point of probabilistic grid specifically include：
Judge the mesh point whether assignment, assigns hit probability value if it is not, whether being hit according to the mesh point or do not order
Middle probability value；
If mesh point assignment, according to following formula update probability value：
Mnew(x)=clamp (odds-1(odds(Mold(x))·odds(phits)))；
Mnew(x)=clamp (odds-1(odds(Mold(x))·odds(pmiss)))。
4. a kind of service robot air navigation aid according to claim 1, which is characterized in that it is described using quickly traversal with
The step of machine tree algorithm carries out global path planning to the map of structure specifically includes：
Random tree is guided to be grown towards target direction by quickly traversing random tree algorithm in the introducing of Artificial Potential Field Method path planning；Institute
State introduce quickly traverse random tree algorithm formula it is as follows：
5. a kind of service robot air navigation aid according to claim 1, which is characterized in that the local avoidance of utilization
The step of dynamic window algorithm carries out local paths planning to the map of structure specifically includes：
The current state of service robot is sampled；
The travel route after the service robot preset time is calculated for each sample rate；
It is scored a plurality of travel route using default evaluation criterion；
Path is selected according to appraisal result.
6. a kind of service robot navigation system, which is characterized in that including：
Error concealment module eliminates what map structuring generated in the process for being detected by closed loop using cartographer algorithms
Accumulated error；
Global path planning module, for carrying out global path planning to the map of structure using quickly traversing random tree algorithm；
Local paths planning module carries out local path rule for the dynamic window algorithm using local avoidance to the map of structure
It draws.
7. a kind of service robot navigation system according to claim 1, which is characterized in that the error concealment module tool
Body includes：
Sub- map establishes unit, the mesh point for establishing the sub- map that closed loop detects and the probabilistic grid for assigning the sub- map
Probability value；
Local matching scanning element, for the pose using Ceres-based matching scanning information points relative to current sub- map
To find the pose in the scanning information point set of the maximum probability of the sub- map；
Global closed loop optimizes unit, and the pose for optimizing all scanning information points and sub- map using SPA simultaneously will be described opposite
Pose is stored；
Accumulated error eliminates unit, for carrying out asking for closed loop detection and relative pose using branch-and-bound and DFS search strategies
Solution is to eliminate accumulated error.
8. a kind of service robot navigation system according to claim 7, which is characterized in that it is specific that sub- map establishes unit
For judging the mesh point whether assignment, assigns hit probability value if it is not, whether being hit according to the mesh point or do not order
Middle probability value；If mesh point assignment, according to following formula update probability value：
Mnew(x)=clamp (odds-1(odds(Mold(x))·odds(phits))；
Mnew(x)=clamp (odds-1(odds(Mold(x))·odds(pmiss)))。
9. a kind of service robot navigation system according to claim 6, which is characterized in that the global path planning mould
Block specifically includes：
Unit is introduced, for guiding random tree towards mesh by quickly traversing random tree algorithm in the introducing of Artificial Potential Field Method path planning
Mark direction growth；The formula that the introducing quickly traverses random tree algorithm is as follows：
10. a kind of service robot navigation system according to claim 6, which is characterized in that the local paths planning
Module specifically includes：
Sampling unit is sampled for the current state to service robot；
Computing unit, for calculating the travel route after the service robot preset time for each sample rate；
Score unit, for being scored a plurality of travel route using default evaluation criterion；
Selecting unit, for selecting path according to appraisal result.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201810058588.4A CN108375976A (en) | 2018-01-22 | 2018-01-22 | A kind of service robot navigation methods and systems |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201810058588.4A CN108375976A (en) | 2018-01-22 | 2018-01-22 | A kind of service robot navigation methods and systems |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN108375976A true CN108375976A (en) | 2018-08-07 |
Family
ID=63016520
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201810058588.4A Pending CN108375976A (en) | 2018-01-22 | 2018-01-22 | A kind of service robot navigation methods and systems |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN108375976A (en) |
Cited By (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109407665A (en) * | 2018-09-28 | 2019-03-01 | 浙江大学 | A kind of unmanned dispensing vehicle of small semiautomatic and Distribution path planing method |
| CN109557920A (en) * | 2018-12-21 | 2019-04-02 | 华南理工大学广州学院 | A kind of self-navigation Jian Tu robot and control method |
| CN109753072A (en) * | 2019-01-23 | 2019-05-14 | 西安工业大学 | A kind of mobile robot mixed path planing method |
| CN109782766A (en) * | 2019-01-25 | 2019-05-21 | 北京百度网讯科技有限公司 | Method and apparatus for controlling vehicle driving |
| CN110083167A (en) * | 2019-06-05 | 2019-08-02 | 浙江大华技术股份有限公司 | A kind of path following method and device of mobile robot |
| CN110162063A (en) * | 2019-06-12 | 2019-08-23 | 北京洛必德科技有限公司 | A kind of paths planning method and device for robot automatic charging |
| CN110412596A (en) * | 2019-07-10 | 2019-11-05 | 上海电机学院 | A kind of robot localization method based on image information and laser point cloud |
| CN111077495A (en) * | 2019-12-10 | 2020-04-28 | 亿嘉和科技股份有限公司 | Positioning recovery method based on three-dimensional laser |
| CN111735451A (en) * | 2020-04-16 | 2020-10-02 | 中国北方车辆研究所 | Point cloud matching high-precision positioning method based on multi-source prior information |
Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105867381A (en) * | 2016-04-25 | 2016-08-17 | 广西大学 | Industrial robot path search optimization algorithm based on probability map |
| CN106406320A (en) * | 2016-11-29 | 2017-02-15 | 重庆重智机器人研究院有限公司 | Robot path planning method and robot planning route |
| CN106406338A (en) * | 2016-04-14 | 2017-02-15 | 中山大学 | Omnidirectional mobile robot autonomous navigation apparatus and method based on laser range finder |
-
2018
- 2018-01-22 CN CN201810058588.4A patent/CN108375976A/en active Pending
Patent Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106406338A (en) * | 2016-04-14 | 2017-02-15 | 中山大学 | Omnidirectional mobile robot autonomous navigation apparatus and method based on laser range finder |
| CN105867381A (en) * | 2016-04-25 | 2016-08-17 | 广西大学 | Industrial robot path search optimization algorithm based on probability map |
| CN106406320A (en) * | 2016-11-29 | 2017-02-15 | 重庆重智机器人研究院有限公司 | Robot path planning method and robot planning route |
Non-Patent Citations (3)
| Title |
|---|
| WOLFGANG HESS等: "Real-Time Loop Closure in 2D LIDAR SLAM", 《 2016 IEEE INTERNATIONAL CONFERENCE ON ROBOTICS AND AUTOMATION》 * |
| 白巧克力亦唯心: "机器人局部避障的动态窗口法（dynamic window approach）", 《CSDN》 * |
| 郝利波: "基于一种改进RRT算法的足球机器人路径规划", 《西安科技大学学报》 * |
Cited By (12)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109407665A (en) * | 2018-09-28 | 2019-03-01 | 浙江大学 | A kind of unmanned dispensing vehicle of small semiautomatic and Distribution path planing method |
| CN109557920A (en) * | 2018-12-21 | 2019-04-02 | 华南理工大学广州学院 | A kind of self-navigation Jian Tu robot and control method |
| CN109753072A (en) * | 2019-01-23 | 2019-05-14 | 西安工业大学 | A kind of mobile robot mixed path planing method |
| CN109782766A (en) * | 2019-01-25 | 2019-05-21 | 北京百度网讯科技有限公司 | Method and apparatus for controlling vehicle driving |
| US11378957B1 (en) | 2019-01-25 | 2022-07-05 | Beijing Baidu Netcom Science And Technology Co., Ltd. | Method and apparatus for controlling vehicle driving |
| CN110083167A (en) * | 2019-06-05 | 2019-08-02 | 浙江大华技术股份有限公司 | A kind of path following method and device of mobile robot |
| CN110162063A (en) * | 2019-06-12 | 2019-08-23 | 北京洛必德科技有限公司 | A kind of paths planning method and device for robot automatic charging |
| CN110412596A (en) * | 2019-07-10 | 2019-11-05 | 上海电机学院 | A kind of robot localization method based on image information and laser point cloud |
| CN111077495A (en) * | 2019-12-10 | 2020-04-28 | 亿嘉和科技股份有限公司 | Positioning recovery method based on three-dimensional laser |
| CN111077495B (en) * | 2019-12-10 | 2022-02-22 | 亿嘉和科技股份有限公司 | Positioning recovery method based on three-dimensional laser |
| CN111735451A (en) * | 2020-04-16 | 2020-10-02 | 中国北方车辆研究所 | Point cloud matching high-precision positioning method based on multi-source prior information |
| CN111735451B (en) * | 2020-04-16 | 2022-06-07 | 中国北方车辆研究所 | Point cloud matching high-precision positioning method based on multi-source prior information |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN108375976A (en) | A kind of service robot navigation methods and systems | |
| CN111639811B (en) | Multi-agricultural-machine collaborative operation remote management scheduling method based on improved ant colony algorithm | |
| CN111024092B (en) | Method for rapidly planning tracks of intelligent aircraft under multi-constraint conditions | |
| CN110231833B (en) | Oil field inspection fixed point data acquisition system and method based on multiple unmanned aerial vehicles | |
| KR20210116576A (en) | How to use a visual robot-based map of the past | |
| CN106873599A (en) | Unmanned bicycle paths planning method based on ant group algorithm and polar coordinate transform | |
| CN106767820B (en) | A kind of indoor moving positioning and drafting method | |
| JP5429901B2 (en) | Robot and information processing apparatus program | |
| CN109470254A (en) | Generation method, device, system and the storage medium of map lane line | |
| CN109828578B (en) | Instrument inspection robot optimal route planning method based on YOLOv3 | |
| CN107680403A (en) | Vehicle, which is led, stops method and system | |
| CN109489664A (en) | A kind of method and apparatus of path planning | |
| CN109270927A (en) | The generation method and device of road data | |
| CN110989352A (en) | Group robot collaborative search method based on Monte Carlo tree search algorithm | |
| US20220404836A1 (en) | Method for route optimization based on dynamic window and redundant node filtering | |
| CN112114584A (en) | Global path planning method of spherical amphibious robot | |
| CN106899306A (en) | A kind of track of vehicle line data compression method of holding moving characteristic | |
| CN108645769A (en) | A kind of environmental air quality monitoring method based on unmanned plane | |
| CN112530158B (en) | Road network supplementing method based on historical track | |
| CN106529678A (en) | SLAM data association method based on maximum-minimum ant system optimization | |
| CN111931899A (en) | Network flow prediction method for optimizing extreme learning machine by improving cuckoo search algorithm | |
| CN114859932A (en) | Exploration method and device based on reinforcement learning and intelligent equipment | |
| CN108985488A (en) | The method predicted to individual trip purpose | |
| CN109910015A (en) | The terminal end path planning algorithm of construction robot is smeared in a kind of mortar spray | |
| CN111401611B (en) | Route optimization method for routing inspection point of chemical plant equipment |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| RJ01 | Rejection of invention patent application after publication | ||
| RJ01 | Rejection of invention patent application after publication |
Application publication date: 20180807 |