Research and Implementation of Robot Path Planning Based onVSLAM

. In order to solve the problem of warehouse logistics robots planpath in different scenes, this paper proposes a method based on visual simultaneous localization and mapping (VSLAM) to build grid map of different scenes and use A* algorithm to plan path on the grid map. Firstly, we use VSLAMto reconstruct the environment in three-dimensionally. Secondly, based on the three-dimensional environment data, we calculate the accessibility of each grid to prepare occupied grid map (OGM) for terrain description. Rely on the terrain information, we use the A* algorithm to solve path planning problem. We also optimize the A* algorithm and improve algorithm efficiency. Lastly, we verify the effectiveness and reliability of the proposed method by simulation and experimental results.


Introduction
Using robots in warehouses can greatly improve the efficiency of warehouse transfer, and the path planning of warehousing and logistics robots is the key to improving efficiency.Path planning based on occupied grid map is a common method of environment for robots, such as Dijkstra [1], A* [2], D* [3] and so on.Dijkstra's main application is to find the shortest path between the start and end point of the map, but the path search is nonheuristic and slow.D* mainly solves the problem of the change of travel cost caused by the dynamic change of the environment.For a static environment such as a warehouse, the A* algorithm can find the shortest path between two points more quickly and efficiently [4][5].While a warehouse is a static environment, robots need the ability to rapidly map the environment if they need to be deployed quickly to different warehouses.
Monocular vision SLAM technology can make robots to build maps.There are two types of SLAM visual odometer, which are divided into direct method and feature point method [6].Feature point method mainly includes PTAM [7], ORB-SLAM [8] and other algorithms.Direct methods are mainly LSD-SLAM [9][10], DSO [11][12] and other algorithms.The advantage of the feature point method is that the map drifts small and the loop-closures detection is accurate.However, due to the relatively sparse feature points, it can be used for finding robot localization, difficult to use for robot navigation and path planning, and relativelycalculate slow.Direct method can extract relatively dense environmental features for robot navigation and positioning, and can run in real time on a PC.But the map drift is more terrible than the feature point method, loop-closures detection time is long.This paper mainly considers how to let the robot quickly perceive the environment and complete the path planning.First of all,direct method SLAM are used to construct the 3D point cloud map of the warehouse environment, then makeoccupied grid map according to barrier in 3D map.Secondly, this paper uses A* algorithm to plan the path in the robot's working environment.

2.1Large-scale direct monocular SLAM
Jakob Engel and Daniel Cremers at the Technical University of Munich proposed a monocular SLAM algorithm based on direct method to construct a largescale, globally consistent environment map called Large-Scale Direct (LSD) Monocular SLAM [9][10].Vladyslav-Usenko and Jakob Engel implemented a threedimensional reconstruction of the real-time street environment on the moving cars [13].Jakob Engel, JorgStückler achieved accurate depth estimation and relatively dense three-dimensional reconstruction on a stereo camera [14].For static, stable light conditions indoor, warehouse and other environments, the use of LSD-SLAM can make the robot has a precise positioning and map building capabilities.
The algorithm consists of three major components: tracking, depth map estimation and map optimizationas visualized in Fig. 1: 1.The tracking component continuously tracks new camera images.That is,it estimates their rigid body pose  ∈  3 with respect to the currentkeyframe, using the pose of the previous frame as initialization.Depth is refined by filtering over manyper-pixel, smallbaseline stereo comparisons coupled with interleaved spatialregularization as originally proposed in [9].If the camera has moved too far,a new keyframe is initialized by projecting points from existing, close-bykeyframes into it.

MATEC
3. Once a keyframe is replaced as tracking reference -and hence its depth mapwill not be refined further -it is incorporated into the global map by themap optimization component.To detect loop closures and scale-drift, asimilarity transform  ∈  3 to close-by existing keyframes (includingits direct predecessor) is estimated using scale-aware, direct  3 -imagealignment.

Occupied grid mappreparation
Semi-dense three-dimensional point cloud data can be obtained from the LSD-SLAM algorithm, and the main obstacles in the indoor environment can be identified, but the three-dimensional data still needs to be converted into a two-dimensional grid map to be able to use the A* algorithm for path planning .Suppose a total of  threedimensional data points, recorded as a set:  = {  1  1 ,  1 ,  1 , … ,     ,   ,   } Specific steps are as follows: STEP 1: Filter out point cloud data that formed by objects higher than the robot itself, this objects are hanging on the robot some, such as lights, ceilings, etc., the robot can't reach higher than itself.To ensure that the camera is installed horizontally, according to the pinhole imaging model, the data with the y-axis less than 0 is removed and obtain the point set:  =  , ,   ∈  and  ≥ 0 } Suppose the number of points left nowis, then B can be described as: After making  as  , the grid   will containtotal  points internally, as shown in Fig. 2(a),blue points are clouds points.Select a threshold  1 , when  ≥  1 ,   = 1, which means that the grid can'treach, when  <  1 ,   = 0, means the grid can reach, so that you can make into OGM map, as shown in Fig. 2

Occupied grid mapexpansion
After obtaining OGM map, the robot itself has a certain size, but in the path planning, it is to use a grid to represent the robot, so the robot may collidesome obstacles.It is necessary to expand the obstacles before proceeding with the path planning, update to the OGM map, and then do the pathplanning.For acertain nonaccessible grid, expansion area to the center of the circle,  2 radius of the circular area.

Path planning 3.1 A* algorithm
The A* algorithm is a commonly used heuristic path planning algorithm that can be applied to find the shortest path from the start to the end of a grid map.The algorithm relies on the evaluation function   to measure whether the path is optimal.
=   + () (2) The evaluation function   is an evaluation function of node ,   represents the movement cost evaluation of node  from the starting point, which is the movement distance of all the passed parent nodes by adding n to the starting point; ()is cost to the end point, which is a heuristic value where the Manhattan distance is used to guide the algorithm to find the end point.As shown in Fig. 3, the redletter S indicates the start node, the green letter E indicates the end node and the blue node indicates the found path.Algorithm pseudo-code shown in Fig. 4. if j in op en an d cost < g(j) th en : 13. rem ove j from op en ; 14.
if j not in op en an d not in closed th en : 15.
set father node of j is i; 18.
if count = = 0 th en : if j in op en an d cost < g(j) th en : 13. rem ove j from op en ; 14.
if j not in op en an d not in closed th en : 15.
set father node of j is i; 18.

Occupied grid mappreparation
After obtaining the PLY file, 364413 points cloud data are obtained according to STEP1 of Section 2.2, and then all the points are projected onto the ground to obtain a scattergram.Fig. 7(a) is a path planning software interface running by the industrial computer.The software is based on OpenGL set up to achieve real-time display of 2-D terrain data, the number of horizontal grid is 168, the number of vertical grid is 120, the red point is the point of cloud projection, accounting for one pixel.Progressing the map according to STEP 2 and get the grid map as shown in Fig. 7(b).The red area is not accessible and the green area is the accessible area.There are 8007 unacceptable grids and 121153 grids available.

(c). Expansion OGM
Then the expansion of the grid points is performed, using a Bresenham drawing circle algorithm from computer graphics--using a series of discrete points to approximate the circle and obtaining a grid expansion with a radius of 2 as shown in Fig. 7(c).Radius can be dynamically adjusted

Path Planning Algorithm Simulation
To evaluate the performance and performance of the A* algorithm, the algorithm was tested in the simulation software described above.The author first tests the performance of two data structure A* algorithms at different start node andend node on different size maps of 168 × 120,336 × 240,672 × 480,as shown in Fig. 8

Conclusions
Inthis paper, the grid map preparation method and path planning strategy based on monocular vision simultaneous positioning and map construction are studied.
The environment is reconstructed using the largescale direct method (LSD) model, which enables the environment to be accurately reconstructedin various scenarios of robots.Based on the environment threedimensional data, occupancy grid map (OGM) was prepared for the description of the terrain.According to the terrain information, A * algorithm was used for path planning, and the performance of the algorithm was improved.Simulation shows that the proposed method is more than 5 times faster than the traditional method.
Next research will focus on controlling robot driving along the path and fixing robot pose according to VSLAM (3) pose.

Figure 3 .
Figure 3. A* algorithm find path.In p u t : L ocation of S tart and E n d node O u tp u t : Father node of E nd 1. op en = list containing S tart node; 2. closed = em pty set; 3. m ovecost(x;y) = distance from node x to node y. 4. w h ile E n d node not in op en : d o 5. i = node w ith low est f(i) in op en ; 6. rem ove i from op en ; 7. add i to closed ; 8. count = 0; 9. for j = neighbor node of i an d not in closed an d reachable d o : 10. count+ + ; 11. cost = g(i)+ m ovecost(i;j); 12.if j in op en an d cost < g(j) th en : 13. rem ove j from op en ; 14.if j not in op en an d not in closed th en : 15.add j into op en ; 16. f(j) = g(j)+ h(j); 17.set father node of j is i; 18.if count = = 0 th en : 19.can't ¯nd path; 20.b reak ou t; 19. can't ¯nd path; 20.b reak ou t; In p u t : L ocation of S tart and E n d node O u tp u t : Father node of E nd 1. op en = list containing S tart node; 2. closed = em pty set; 3. m ovecost(x;y) = distance from node x to node y. 4. w h ile E n d node not in op en : d o 5. i = node w ith low est f(i) in op en ; neighbor node of i an d not in closed an d reachable d o : 10. count+ + ; 11. cost = g(i)+ m ovecost(i;j); 12.

Figure 6 (
Figure 6 (a).Indoor room(b).Point clouds map (a) and (b).The yellow grid indicates the path to the plan.The letter S indicates the start node and the letter E indicates the end node.Fig.9 shows the comparison of the three methods at different fifty pairs of start node andend node.The vertical axis shows the fifty pairsaverage time.Unit issecond.It can be seen that the time cost of A* binary heap is about 1/5 of the A* list, while the Dijkstra algorithm has the longest time, ten times as much as the A* list.

Figure 9 .
Figure 9. Vertical axis is average of fifty different pairs of start node and end node summary time cost.Horizontal axis is three different maps.Orange is time cost of A* using binary heap data structure, yellow is time cost of A* using list data structure, green is time cost of Dijkstra algorithm.
The depth map estimation component uses tracked frames to either refineor replace the current keyframe.