Planning Architecture based on the Proposed Planner

As mentioned earlier, by the time the trajectory is generated from the current vehicle state, the vehicle has moved on. A forward simulation of the vehicle system dynamics is therefore necessary for compensating for the planning latency. Given the estimated fu-ture state of the vehicle and the related road model, the spatial horizon can be specified.

General

Bonus is bestowed to the can-didate trajectory whose first from the last plan (cf. Sec-tion 5.3.2), it might fall be-tween the endpoints of an ac-celeration profile. In this case, the trajectory can get a bonus if its first acceleration profile is intended to finish that pro-file.

c^variation_lateral =Pn

i=1(l_i−li−1) wherel_iis the latitude of theith node along the trajec-tory

The variations in the latitudes of the spatial nodes along the trajectory result in penalty.

c^variationacceleration =

−bonus ifα_i ≡αi−1

0 otherwise

whereαi is the index of theith accelera-tion profile along the trajectory.

The variations in acceleration profiles along the trajectory lose bonus.

Safety c^obstacle_static (x, y) =



HIGH-COST if (x,y) is of high proximity to obstacles

HIGH-COST if (x,y, t) is of high proximity to obstacles cost^keeping_distance if (x,y, t) is infollow

area

Table 5.4: Cost functions applied in the proposed planner (part 1).

General

A penalty is given if the longitudinal acceleration the limit is exceeded,∞is assigned.

c_κ˙ =

C_κ˙|κ|˙ if ˙κ is within ˙κ_limit

∞ otherwise Similar to the cost for the

lateral acceleration.

c_κ =

Cκ||κ| − |κ_limit|| ifκ exceeds κ_limit

0 otherwise The trajectory that

ex-ceeds the curvature limit set a hair below the speed limit of the travelling lane.

Table 5.5: Cost functions applied in the proposed planner (part 2).

It is necessary to take some specific measures here in order to guarantee a certain level of planning consistency. These issues will be further discussed in the next two subsections.

Although the scenario reasoning module has not yet been embedded in the current implementation of the planner, it is shown here that it can be seamlessly integrated into the planning strategy. As can be seen from the construction process of the spatial horizon (cf. Section 3.1), a sequence of edges encoding the road information can be obtained as a by-product of the spatial horizon construction. After the distance horizon confining the spatial horizon is reached, the search for the edges can go on without further construction and update of the LATTICE-SEC-RAWs. Given the edges that extend beyond the planning horizon, some long-term (from the perspective of the limited planning horizon) information can be gathered. Such information is used by the scenario reasoning module to set target speeds and schedule the weights of the cost functions.

Following those instructions, the motion planner can make more far-sighted decisions.

Figure 5.10: Planning system of MIG. Left: the existing planning architecture.

Right: the proposed planner-based planning modules necessary for replacing the ones inside the dashed box on the left.

Figure 5.11 shows an example scenario related to this issue, where the information of the traffic light can be used to calculate a target speed for the planning within the planning horizon. Under the guidance of the target speed, the planner can generate a more smooth trajectory of deceleration.

Planning horizon Scenario reasoning horizon

Figure 5.11: A driving scenario where the traffic light information extracted from the edges that extend beyond the planning horizon can assist the short-sighted planner in making far-short-sighted plans.

Given the sampled spatial horizon, the scenario-specific target speed and weights of the cost functions, the world model and the start state of the vehicle, it is ready for the planner to compute the best constraint-abiding trajectory. The resultant trajectory is sampled into a sequence of vector samples {(κ(t), v(t), α(t))}. These vector samples are then sent to the controller where the actuation commands are calculated. Both the planner and the controller maintain a queue of trajectories sampled in this way. The planner uses the trajectory queue to perform latency compensation, as is described later in Section5.3.2. The controller extracts the desired states (ˆκ(tcur),ˆv(tcur),α(tˆ cur)) from the most recent trajectory that is valid over the current time t_cur. Based on the desired states and the current states (κ, v, α) of the vehicle, the controller calculates the steering and throttle/brake actuation commands (φc, αc) according to:

φ_c = −arcsin(Lˆκ)

αc = kp(ˆv−v) +k_d(α_{dif f}) +k_{f f}αˆ (5.3) where L refers to the distance between the vehicle’s front and rear axles, k_i, k_p are the parameters of the PI controller, kf f is the gain of the feedforward controller, and α_{dif f} is the discrepancy between the desired acceleration ˆαand the predicted acceleration outcome. The prediction is based on the acceleration model identified in [88] and applied currently in the simulator. It can be seen from Equation 5.3 that the acceleration controller is a PI plus feedforward controller. The calculation of the steering actuation command φc follows a nonholonomic vehicle model, i.e, the bicycle model shown in Figure 5.12, which is also applied in the simulator for the simulation of the lateral movement of the vehicle. It is noteworthy that the trajectory generated by the proposed planner describes the expected locus of the middle of the front axle of the vehicle. That is the reason whyarcsin, rather thanarctan, is used in the calculation of φ_c.