MPC

정승균·2021년 2월 4일

자율주행 제어

목록 보기

6/8

\mathbf{x} = \begin {bmatrix} x & y & v & \psi & \delta \end {bmatrix}^T

\mathbf{u} = \begin {bmatrix} a & \omega \end {bmatrix}^T

\dot x = v\cos \psi \\ \dot y = v\sin \psi \\ \dot v = a \\ \dot \psi = v\frac{tan\delta}{L} \\ \dot \delta = \omega \\

\dot {\mathbf{x}} = {\mathbf{A}}{\mathbf{x}} +{\mathbf{B}}{\mathbf{u}}

{\mathbf{A}} = \begin {bmatrix} 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 \end {bmatrix}

|a| \le a_{max} \\ |\delta| \le \delta_{max} \\ |\omega| \le \omega_{max} \\

\min \quad (\mathbf{x} - \mathbf{x}_{ref})^T \mathbf{Q} \; (\mathbf{x} - \mathbf{x}_{ref}) + \mathbf{u}^T \mathbf{R} \; \mathbf{u}

{\mathbf{Q}} = \begin {bmatrix} q_{11} & 0 & 0 & 0 & 0 \\ 0 & q_{22} & 0 & 0 & 0 \\ 0 & 0 & q_{33} & 0 & 0 \\ 0 & 0 & 0 & q_{44} & 0 \\ 0 & 0 & 0 & 0 & q_{55} \end {bmatrix} \quad {\mathbf{R}} = \begin {bmatrix} r_{11} & 0\\ 0 & r_{22}\\ \end {bmatrix}

\min {x(\cdot),u(\cdot),p} & & \int_{t_0}^{t_0 + T} \\ \Vert h( t, x(t), u(t), p ) - \eta(t) \Vert_Q^2 \; \mathrm{d}t + \Vert m( x(t_0+T), p, t_0+T ) - \mu \Vert_P^2 \\ \textrm{subject to:} & & x(t_0) = x_0 \\ \forall t \in [t_0, t_0 + T]: & & 0 = f( t, x(t), \dot x(t), u(t), p ) \\ \forall t \in [t_0, t_0 + T]: & & 0 \geq s( t, x(t), u(t), p ) \\ & & 0 = r( x(t_0+T), p, t_0+T )