This paper presents an optimization-based approach for robotic motion planning in a static and complex environment. This method designs optimization functions and constraint in-equalities based on requirements, and then we use the functional gradient technique to improve the quality of the initial trajectory iteratively. It uses the numerical distance information between the environment’s point cloud and the robot’s boundary to describe the collision tendency. Constraint inequalities that are difficult to describe are handled by setting a threshold value γ, a maximum penetration depth point, and a bounding volume hierarchy. Finally, We do some experiments with this method on a six-degree-of-freedom manipulator UR3. Experiments demonstrate that the performance of the manipulators is improved, and the trajectory is effectively optimized while avoiding collisions with non-convex obstacles.