Road safety is one of the major concerns for automated vehicles. In order for these vehicles to interact safely and efficiently with the other road participants, the behavior of the automated vehicles should be carefully designed. Liu and Tomizuka proposed the Robustly-safe Automated Driving system (ROAD) which prevents or minimizes occurrences of collisions of the automated vehicle with other road participants while maintaining efficiency. In this paper, a set of design principles are elaborated as an extension of the previous work, including robust perception and cognition algorithms for environment monitoring and high level decision making and low level control algorithms for safe maneuvering of the automated vehicle. The autonomous driving problem in mixed traffic is posed as a stochastic optimization problem, which is solved by 1) behavior classification and trajectory prediction of other road participants, and 2) a unique parallel planner architecture which addresses the efficiency goal in the long term and the safety goal in the short term separately. Moreover, a python- based high fidelity simulation system is developed and extensive simulations are performed to evaluate the effectiveness of the proposed algorithm, where both high level decision making and low level vehicle regulation are considered. Two typical scenarios are studied, driving on freeway and driving in unstructured environments such as parking lots. In the simulation, multiple moving agents representing surrounding vehicles and pedestrians are added to the environment, some of which are controlled by human subjects in order to test the real time response of the automated vehicle.