A numerical method consisting of an off-line part and an on-line part for optimal control problems is proposed in this paper. In the off-line part, the state space is discretized into a Cartesian grid structure and then define a graph over all grid points by connecting two points if the Euclidean norm between them is closer than a positive number called adjacent radius, the minimum cost between them is estimated using difference method and stored in a matrix. After that the matrix is updated by a shortest path algorithm and a matrix holding the information of the shortest paths between any two grid points is generated. In the on-line part, the optimal control vector at each time step can be generated by reading data from the matrix according to the current state and target state and doing some simple calculations. Since there is no need to do a lot of calculation in the on-line part, this method can satisfy the real-time requirements in some engineering control problems. We prove that the solution of the proposed method converge to the analytical solution when the adjacent radius and the grid size tend to zero and the grid size tend is a higher order infinitesimal of the adjacent radius. At the end of this paper, some numerical examples are taken to illustrate the effectiveness of the proposed method.