自动驾驶运动规划

Problem Description

The vehicle motion planning problem is stated as follows. Minimize the path deviation cost functional

$$\begin{align} J = \int_{0}^{t_f} \omega_x (x - x_{\text{ref}})^2 + \omega_y (y - y_{\text{ref}})^2 + \omega_{v} (v - v_{\text{ref}})^2 dt \tag{1} \end{align}$$

subject to the dynamic constraints:

$$\begin{align} \dot{x} &= v \cos \phi \tag{2.1} \\ \dot{y} &= v \sin \phi \tag{2.2} \\ \dot{\phi} &= \frac{v \tan \delta}{l} \tag{2.3} \\ \dot{v} &= a \tag{2.4} \end{align}$$

boundary conditions:

$$\begin{align} x(0) = x_0, \ & x(t_f) = \text{Free} \tag{3.1} \\ y(0) = y_0, \ & y(t_f) = \text{Free} \tag{3.2} \\ \phi(0) = \phi_0, \ & \phi(t_f) = \text{Free} \tag{3.3} \\ v(0) = v_0, \ & v(t_f) = \text{Free} \tag{3.4} \end{align}$$

Note

One of the main capabilities of autonomous driving motion planning is the ability to model and solve precise obstacle avoidance. OPTIMake is supporting implicit function modeling based on occupancy fields, which can achieve differentiable processing flows internally.

Modeling

Python

prob = multi_stage_problem('kinematic', 21)

xweight, yweight, vweight = prob.parameters(['xweight', 'yweight', 'vweight'])
xref, yref, vref = prob.parameters(['xref', 'yref', 'vref'], stage_dependent=True)
x0, y0, phi0, v0 = prob.parameters(['x0', 'y0', 'phi0', 'v0'], stage_dependent=False)

x = prob.variable('x')
y = prob.variable('y')
phi = prob.variable('phi')
delta = prob.variable('delta', hard_lowerbound=-0.2, hard_upperbound=0.2)
v = prob.variable('v')
a = prob.variable('a', hard_lowerbound=-1.0, hard_upperbound=1.0)

obj = general_objective(xweight * (x - xref)**2 + yweight * (y - yref)**2 + vweight * (v - vref)**2)
prob.objective(obj)

""" ode """
length = 1.0
ode = differential_equation(
    state=[x, y, phi, v], 
    state_dot=[v * cos(phi), v * sin(phi), v * tan(delta) / length, a], 
    stepsize=0.5, 
    discretization_method='erk4')
prob.equality(ode)

seq = general_equality([x - x0, y - y0, phi - phi0, v - v0])
prob.start_equality(seq)

option = codegen_option()

codegen = code_generator()
codegen.codegen(prob, option)

Solution

the solution to the vehicle motion planning problem using OPTIMake is shown in the figures below.