軌跡追従制御をやってみた

はじめに

こちらの記事では、制御工学に基づき簡易的な自動車の軌跡追従制御を実施したものです。

昨今、自動運転にはモデル予測制御(MPC)が多く適用されていますが、本記事ではMPCは使用しません。
MPCを用いた軌跡追従制御に関してはモデル予測制御（MPC）による軌道追従制御をご参考ください。

動作環境

・MATLAB R2022a Update1
・Simulink R2022a

制御対象（プラント）のモデル化

自動車のモデリングに関しては、様々なものが提案されていますが、今回は簡易的な等価二輪モデルを採用します。
以下は制御対象の概要図です。

パラメータ	概要
$(x,y)$	ワールド座標系における後輪座標
$(x_f,y_f)$	ワールド座標系における後輪座標
$l$	前後輪幅
$v$	ワールド座標系における車速
$\theta$	ワールド座標系における車両角
$\delta$	車両に対するタイヤ角

ここで、状態を式(1)のように定義します。

\boldsymbol{x} =
\begin{pmatrix}
x\\ y\\ \theta
\end{pmatrix} \tag{1}

入力は式(2)で定義します。

\boldsymbol{u} =
\begin{pmatrix}
v\\ \delta
\end{pmatrix} \tag{2}

出力は式(3)で定義します。

\boldsymbol{y} =
\begin{pmatrix}
x \\ y
\end{pmatrix} \tag{3}

式(1)~(3)より状態方程式は式(4)で表現されます。

\begin{align}
\frac{d\boldsymbol{y}}{dt} = 
\begin{pmatrix}
v\cos \theta \\ v\sin \theta \\ \frac{v}{l} \tan \delta
\end{pmatrix} \tag{4}
\end{align}

式(4)を見ると、三角関数が使用されており非線形であることがわかります。

制御則の構築

上述しましたが、制御対象は非線形であるため、制御則の構築には工夫が必要です。
まず制御対象に対して入出力線形化を施します。線形化については以下をご参考ください。
・状態方程式の厳密な線形化（東工大三平研究室）
・システム制御理論特論（北大）

式(3)の出力を微分します。

\begin{align}
\frac{d\boldsymbol{y}}{dt} = 
\underbrace{
\begin{pmatrix}
\cos \theta && 0 \\ \sin \theta && 0
\end{pmatrix} 
}_{\boldsymbol{R} }
\boldsymbol{u} \tag{5}
\end{align}

ここで行列$ \boldsymbol{R}$は正則でない（逆行列をもたない）ため、出力$\boldsymbol{y}$の取り方を車両後輪から車両前輪へ変えます。

\begin{align}
\boldsymbol{y} &=
\begin{pmatrix}
x_f \\ y_f
\end{pmatrix}
=
\begin{pmatrix}
x+l\cos \theta \\ y+l \sin \theta
\end{pmatrix} \\
\frac{d\boldsymbol{y}}{dt} &=
\begin{pmatrix}
\dot{x}-l \sin \theta \dot{\theta} \\
\dot{y}+l\cos \theta \dot{\theta}
\end{pmatrix}  \\
&=
\begin{pmatrix}
v \cos \theta -l \sin \theta \frac{v}{l} \tan \delta \\
v \sin \theta +l \cos \theta \frac{v}{l} \tan \delta
\end{pmatrix}  \\
&=
\underbrace{
\begin{pmatrix}
\cos \theta && -\sin \theta \\
\sin \theta && \cos \theta
\end{pmatrix} }_{\boldsymbol{R}}
\begin{pmatrix}
v \\
v \tan \delta
\end{pmatrix} 
\end{align} \tag{6}

行列$\boldsymbol{R}$は正則になります。

ここらは制御則を構築していきます。
まず軌跡追従ということで、目標となる前輪座標と実際の前輪座標との偏差を式(7)のように定義します。

\boldsymbol{e} = 
\begin{pmatrix}
x_r - x_f \\
y_r - y_f
\end{pmatrix} \tag{7}

ここで

\begin{align}
\frac{d \boldsymbol{e}}{dt} &= -K\boldsymbol{e} \tag{8} \\
\Rightarrow　\boldsymbol{e} &= \exp (-Kt)
\end{align}

となり、$K>0$とすれば偏差$\boldsymbol{e}$は0に収束することになります。
式(6)~(8)より

\begin{align}
\begin{pmatrix}
\dot{x_r} - \dot{x_f} \\
\dot{y_r} - \dot{y_f}
\end{pmatrix}
&=
-K
\begin{pmatrix}
x_r - x_f \\
y_r - y_f
\end{pmatrix} \\
\Rightarrow
\boldsymbol{R}
\begin{pmatrix}
v \\ v \tan \delta
\end{pmatrix} 
&=
\begin{pmatrix}
\dot{x_r} \\ \dot{y_r} 
\end{pmatrix}
+K
\begin{pmatrix}
x_r - x_f \\
y_r - y_f
\end{pmatrix} \\
\Rightarrow
\begin{pmatrix}
v \\ v \tan \delta
\end{pmatrix}
&=
\boldsymbol{R}^{-1} {
\begin{pmatrix}
\dot{x_r} + K(x_r - x_f) \\
\dot{y_r} + K(y_r - y_f)
\end{pmatrix} } \tag{9}
\end{align}

と表現することができます。
式(2)より車両への入力は車速$v$とタイヤ角$\delta$であるため、式(9)より

\boldsymbol{u} =
\begin{pmatrix}
v \\ \delta
\end{pmatrix}
=
\begin{pmatrix}
\cos \theta (\dot{x_r} + K(x_r - x_f)) + \sin \theta (\dot{y_r} + K(y_r - y_f)) \\
\tan^{-1} \frac{-\sin \theta (\dot{x_r} + K(x_r - x_f)) + \cos \theta (\dot{y_r} + K(y_r - y_f))}{v}
\end{pmatrix} \tag{10}

と表現しなおします。
Simulinkブロックは以下のようにしました。

シミュレーション

式(10)で制御則を表現することができました。
ここからはMATLAB/Simulinkを用いてシミュレーションを実施していきます。
シミュレーションに際し、設計したパラメータは以下のようにしました。

パラメータ	値
$K$	0.1
$l$	2.69

シミュレーション結果は以下です。
完璧ではないですが、おおよそ目標軌跡に追従できていることが確認できます。

まとめ

MPCを使用せずに軌跡追従制御を実施しました。線形化にために座標変換が必要であったりと少々面倒ですが、世の中の制御対象は非線形なものが多いと思いますので、大切な考え方であると改めて感じさせられました。
また、今回は簡易的な制御とシミュレーションを実施しましたが、実際の制御では入力信号の制限など制約下でのアルゴリズム開発が必要になってきます。その点においてMPCは制約下で最適化問題を解くため非常に有効的と言われていますね。