## 4 Motion Planning and Control in ADAS for City Buses
### Path Planning in Restricted Areas  
In the prototype ADAS solution, thanks to the specific properties of the docking manoeuvre, path planning was facilitated by analytical (geometrical) relations, and for more difficult manoeuvres, the state lattices approach [52] was utilised. This was made possible by formulating the path-planning problem as a graph search problem where a predefined set of path primitives is expanding recursively until the optimum path is found [34]. This planner samples from a finite set of motion primitives that could lead to the generation of a suboptimal path, especially in terms of steering wheel movement, which was difficult for the human driver to follow. Therefore, for the ADAS variant evaluated by MPK Poznari, a more general approach to path planning under various constraints was desired. The method for path planning, including monotonic docking or parking manoeuvres, should take into account kinematic constraints while not being limited to the use of specific curves. Such a method was implemented based on the non-linear optimisation of coefficients of a 7 th-degree polynomial defined as the reference path. The path-planning objective function is subjected to the initial and final configuration of the bus, the kinematic constraints, which are defined as curvature and rate of curvature (2), and the constraints resulting from obstacles. The advent of efficient interior point optimisation methods has enabled a tractable solution to large-scale linear and non-linear programming problems. Considering this fact, our planner is implemented taking advantage of the CasADi software [53] that utilises the IPOPT (Interior Point OPTimizer) [54] library for the large-scale non-linear optimisation of continuous systems to solve the optimisation problem in path planning. As the planner must provide an obstacle-free path, it needs to consider a local map of the environment around the electric charger's site. Although our earlier approach to path planning used a local grid map of the environment, the new variant avoids the direct use of an occupancy grid because of two reasons. First, the version of ADAS implemented for the presented evaluation does not use any external sensors; thus, it cannot build an occupancy map online. Second, a detailed (high-resolution) occupancy grid made path planning slower. Hence, we take advantage of the new formulation of the planning problem as optimisation with equality and inequality constraints, and we transform a local map of the scene around the charger, together with the bus and the charger's configurations, into a set of constraints for optimisation. These inequality constraints define the allowed area of motion for the bus while executing the manoeuvre. As our ADAS cannot build its own map of the environment, it leverages the available internet access and accurate position information from RTK GNSS to obtain the layout of objects around the charger from the OpenStreetMaps free service. The downloaded local map is then used to generate automatically multiply bounding boxes (Figure 10) using Algorithm 1. These boxes provide positional inequality constraints of the control points placed on the polynomial. ```
Input: initial and final configurations: \(P_{0}(x_{0},y_{0},\theta_{0})\) and \(P_{F}(x_{F},y_{F},\theta_{F})\). function CalculateWeight\((p_{1},p_{2})\) \(h\leftarrow\sqrt{(x_{p_{2}}-x_{p_{1}})^{2}+(y_{p_{2}}-y_{p_{1}})^{2}}\) while obstacle free do  Expand the rectangle normal to the line segments \(h\) by \(\Delta w\). az \(w\gets w+\Delta w\) endwhile return\(h,w\) endfunction  Calculate \(y=m_{0}x+b_{0}\) and \(y=m_{F}x+b_{F}\) lines along \(P_{0}\) and \(P_{F}\). \(P_{c}\leftarrow(x_{c},y_{c})\)\(\triangleright\) Calculate intersection point of two lines \(rec_{1}(h_{1},w_{1})\leftarrow\) CalculateWeight\((P_{0},P_{c})\) \(rec_{3}(h_{3},w_{3})\leftarrow\) CalculateWeight\((P_{c},P_{F})\) \(rec_{2}(h_{2},w_{2})\leftarrow\) CalculateWeight\((P_{c}+w_{1}/2,P_{c}-w_{1}/2)\) Output: The height \(h\) and weight \(w\) of each rectangles \(rec1,rec2,rec3\).