4.4 KiB

Raw Blame History

Model Predictive Control for Vehicle model

This program is for controlling the vehicle model. I implemented the steering control for vehicle by using Model Predictive Control.

Model

Usually, the vehicle model is expressed by extremely complicated nonlinear equation. Acoording to reference 1, I used the simple model as shown in following equation.

$\begin{align*} x[k+1] = x[k] + v\cos(\theta[k])dt \\ y[k+1] = y[k] + v\sin(\theta[k])dt \\ \theta[k+1] = \theta[k] + \frac{v \tan{\delta[k]}}{L}dt\\ \delta[k+1] = \delta[k] - \tau^{-1}(\delta[k]-\delta_{input})dt \end{align*}$

However, it is still a nonlinear equation. Therefore, I assume that the car is tracking the reference trajectory. If we get the assumption, the model can turn to linear model by using the path's curvatures.

$\boldsymbol{X}[k+1] = \begin{bmatrix} 1 & vdt & 0 \\ 0 & 1 & \frac{vdt}{L \ cos^{2} \delta_r} \\ 0 & 0 & 1 - \tau^{-1}dt\\ \end{bmatrix} \begin{bmatrix} y[k] \\ \theta[k] \\ \delta[k] \end{bmatrix} + \begin{bmatrix} 0 \\ 0 \\ \tau^{-1}dt \\ \end{bmatrix} \delta_{input} - \begin{bmatrix} 0 \\ -\frac{v \delta_r dt}{L \ cos^{2} \delta_r} \\ 0 \\ \end{bmatrix}$

and \delta_r denoted

$\delta_r = \arctan(\frac{L}{R})$

R is the curvatures of the reference trajectory.

Now we can get the linear state equation and can apply the MPC theory.

However, you should care that this state euation could be changed during the predict horizon. I implemented this, so if you know about the detail please go to "IteraticeMPC_func.py"

Expected Results

Usage

$ python main_track.py

Reference

1. https://qiita.com/taka_horibe/items/47f86e02e2db83b0c570#%E8%BB%8A%E4%B8%A1%E3%81%AE%E8%BB%8C%E9%81%93%E8%BF%BD%E5%BE%93%E5%95%8F%E9%A1%8C%E9%9D%9E%E7%B7%9A%E5%BD%A2%E3%81%AB%E9%81%A9%E7%94%A8%E3%81%99%E3%82%8B (Japanese)

4.4 KiB Raw Blame History