In pursuit of enhancing driving safety while controlling and reducing vehicle and testing costs, our research explores advanced decision logic for autonomous driving. This study integrates foundational algorithms and employs a suite of tools, including MATLAB, Simulink, Prescan, and CarSim, to develop a comprehensive virtual simulation testing environment. Within this virtual platform, we create and test various environmental parameters and vehicle states to simulate real-world driving conditions accurately. Our core cost-based decision-making algorithm processes perceptual results to calculate the “cost,” which then informs driving decisions and planning. By default, the planning tried not to decelerate, but maintained acceleration changes as smooth as possible, which leading to unsafe scenarios such as driving off-road to go around the obstacles. In contrast, we proposed an explainable algorithm that could effectively balances safety and comfort, ensuring safer driving outcomes. Furthermore, our work could contribute to the field of Explainable Artificial Intelligence in Autonomous Driving by providing a robust framework for testing and optimizing decision logic, ultimately leading to the development of safe and efficient autonomous vehicles.