Embodiments herein relate to an autonomous vehicle or self-driving vehicle. The system can determine a collision avoidance path by: 1) predicting the behavior/trajectory of other moving objects (and identifying stationary objects); 2) given the driving trajectory (issued by autonomous driving system) or predicted driving trajectory (human), establishing the probability for a collision that can be calculated between the vehicle and one or more objects; and 3) finding a path to minimize the collision probability.