I finished the way point smoother optimization algorithm to replace B-spline curve algorithm to make sure the final smoother path will not collide with obstacles.
The failure cases:
After way point smoother optimization:
Other cases using way point smoother optimization: