This paper presents a novel approach to process 3D point cloud data to create a 3D grid evaluation map for path planning algorithm. In particular, it is aimed at solving the 3D navigation problem by using raw point cloud as path algorithm's input. After data processing, the gained 3D grid map is able to be implemented to path planning algorithms and allowing planning a safe and optimal path in it via repeated computation of nearby nodes. The proposed approach will integrate environment survey, environment reconstruction, and path planning as a completed task, which involves learning and planning. A competed simulation of the environment topology acquisition and route planning process is documented in this paper.