Planning safe trajectories in Autonomous Driving Systems (ADS) is a complex problem to solve in real-time. The main challenge to solve this problem arises from the various conditions and constraints imposed by road geometry, semantics and traffic rules, as well as the presence of dynamic agents. Recently, Model Predictive Path Integral (MPPI) has shown to be an effective framework for optimal motion planning and control in robot navigation in unstructured and highly uncertain environments. In this paper, we formulate the motion planning problem in ADS as a nonlinear stochastic dynamic optimization problem that can be solved using an MPPI strategy. The main technical contribution of this work is a method to handle obstacles within the MPPI formulation safely. In this method, obstacles are approximated by circles that can be easily integrated into the MPPI cost formulation while considering safety margins. The proposed MPPI framework has been efficiently implemented in our autonomous vehicle and experimentally validated using three different primitive scenarios. Experimental results show that generated trajectories are safe, feasible and perfectly achieve the planning objective. The video results as well as the open-source implementation are available at: https://gitlab.uni.lu/360lab-public/mppi
Automated Driving Systems (ADS) have rapidly evolved in recent years and their architecture becomes sophisticated. Ensuring robustness, reliability and safety of performance is particularly important. The main challenge in building an ADS is the ability to meet certain stringent performance requirements in terms of both making safe operational decisions and finishing processing in real-time. Middlewares play a crucial role to handle these requirements in ADS. The way middlewares share data between the different system components has a direct impact on the overall performance, particularly the latency overhead. To this end, this paper presents FastCycle as a lightweight multi-threaded zero-copy messaging broker to meet the requirements of a high fidelity ADS in terms of modularity, real-time performance and security. We discuss the architecture and the main features of the proposed framework. Evaluation of the proposed framework based on standard metrics in comparison with popular middlewares used in robotics and automated driving shows the improved performance of our framework. The implementation of FastCycle and the associated comparisons with other frameworks are open sourced.