走行距離を時間軸とする時間軸状態制御形

目次

 はじめに

自動車などの経路追従制御でよく利用される2輪モデルは非ホロノミック(Nonholonomic)な拘束を有するシステムであり，比較的制御が困難であると言われています．

このようなシステムを安定化する方法として，以下のような手法が挙げられています．

• 時変状態フィードバック
• 不連続フィードバック
• 時間軸状態制御

最近は時間軸状態制御に基づく手法をよく見るような気がしますので，勉強までに実装してみました．

ベースとする資料は下記の論文です．

www.jstage.jst.go.jp

時間軸状態制御の概要

制御系を実現する際には状態フィードバックのようなフィードバック系を利用することが多いです．
このようなフィードバック系は静的連続状態フィードバック系と呼ばれます．
しかし非ホロノミック系はこのようなフィードバック系では安定化できないことが知られています．

時間軸状態制御(Time state control)では，座標変換により「新たな時間軸」を定義し，
その「新たな時間軸」に対するフィードバック系を設計します．

このとき設計するフィードバック系は通常の状態フィードバック系でよいため，
従来の設計手法や制御則を利用できる点が大きな利点です．

応用として時間軸状態制御形に変換後，モデル予測制御を適用した例などもあります．
今回のベースとする論文も非線形のモデル予測制御を適用しています．

また，2輪モデルへの適用の場合には実装が極めて簡単なところも利点と言えます．

時間軸の選定

「新たな時間軸」には様々な選択肢がありますが，私が見たところでは大きく3つに分類されます．

• 軸（平面上に固定した軸）を時間軸にする
• 目標経路（参照経路）を時間軸にする
• 走行経路（自車の走行距離）を時間軸にする

軸を利用する方法は，発案者である三平先生（東京工業大学）が最初に利用した手法です．
この手法では以上の方位姿勢変化がある場合には座標系を切り替える必要があるようで，
現在ではあまり利用されていないようです．

その後，目標経路を利用する方法と走行経路を利用する方法が出てきていますが，
後者の方が新しいアプローチのようです．

今回ベースとする論文が，走行経路を時間軸とする手法を提案した論文でもあります．

適用の流れ

時間軸制御形を適用する際には，以下の流れが必要となります．

1. 新しい時間軸を定義する
2. 新しい状態ベクトルを定義する
3. 決定した時間軸と状態ベクトルを利用してシステムを座標変換する
4. 座標変換後のシステムに対してフィードバック系を設計する

今回，時間軸は自車の走行距離に取ると決めています．
また，フィードバック系の設計については従来の手法を利用することができます．

重要なところは状態ベクトルの定義とシステムの座標変換の部分となります．

座標変換の到着点

通常，システムの状態方程式を立てる際には，状態ベクトルは時間微分の形式で取ります．

\begin{align}
\frac{d}{dt} \left[\begin{array}{c}
x \\
\displaystyle\frac{dx}{dt}
\end{array}\right] = \left[\begin{array}{cc}
a_{11} & a_{12} \\
a_{21} & a_{22}
\end{array}\right] \left[\begin{array}{c}
x \\
\displaystyle\frac{dx}{dt}
\end{array}\right] + \left[\begin{array}{c}
b_{1} \\
b_{2}
\end{array}\right] u
\end{align}

時間軸状態制御形では，新たに定義する時間軸に対する微分の形式で取ることが特徴です．

\begin{align}
\frac{d}{ds} \left[\begin{array}{c}
x \\
\displaystyle\frac{dx}{ds}
\end{array}\right] = \left[\begin{array}{cc}
a'_{11} & a'_{12} \\
a'_{21} & a'_{22}
\end{array}\right] \left[\begin{array}{c}
x \\
\displaystyle\frac{dx}{ds}
\end{array}\right] + \left[\begin{array}{c}
b'_{1} \\
b'_{2}
\end{array}\right] u'
\end{align}

あとはこのシステムに対して状態フィードバック系を設計するだけなので，
上の形式に変換することが時間軸状態制御形を扱う上でのゴールとなります．

時間軸状態制御の2輪モデルへの適用

2輪モデル

2輪モデルの運動は以下の式で表すことができます．
よく知られている一般的な式です．

\begin{align}
\dot{x} = v \cos\theta \\
\dot{y} = v \sin\theta \\
\dot{\theta} = \frac{v}{l} \tan\delta
\end{align}

ここで，，，はそれぞれ車両後輪位置および姿勢角です．
また，は車両の速度，は車両操舵角，はホイールベースを表します．

新しい時間軸と状態ベクトルを定義する

今回は，新しい時間軸を「自車の走行距離」に取ると決まっているため，
まずは自車の走行距離をと定義します．
また，ここから明らかにの時間微分となることがわかります．

次に，新しい状態を参照経路から車両後輪位置までの距離として定義します．
新しい状態の微分は自車の走行距離で行われるため，となり，以下の式となります．

\begin{align}
\frac{dz}{ds} = \frac{dz}{dt} \frac{dt}{ds}
\end{align}

また，2階微分は同様に以下の式で得られます．

\begin{align}
\frac{d^2z}{ds^2} = \frac{d}{ds}\left( \frac{dz}{dt} \frac{dt}{ds} \right)
\end{align}

上式の中ではのみが未知なため，これを幾何学的関係から導出します．

発見的仮定

• 新しい時間軸を自車の走行距離に取る．

• 新しい状態を参照経路から車両後輪位置までの距離に取る．

• 新しい状態に対する2階微分まで利用する．

幾何学的関係の導出

論文中では基底幾何ベクトル表現となっていますが，あまり馴染みがないので
代数ベクトル表現で書き直しています．

慣性座標系において，位置ベクトル，，は
以下の関係にあります．

\begin{align}
q_{r} = q + p
\end{align}

また，この時間微分は以下のとおりです．

\begin{align}
\dot{q_{r}} = \dot{q} + \dot{p}
\end{align}

 ここで左辺はを参照経路上の速度として，

\begin{align}
\dot{q_{r}}=\left[\begin{array}{c}
\dot{s_{r}} \cos\theta_{r} \\
\dot{s_{r}} \sin\theta_{r}
\end{array}\right]
\end{align}

右辺第1項は，

\begin{align}
\dot{q}=\left[\begin{array}{c}
v \cos\theta \\
v \sin\theta
\end{array}\right] .
\end{align}

状態は参照経路から車両後輪位置までの距離に取るため，位置ベクトルに沿った状態となります．
ただし，向きは反対なため負号がついています．

\begin{align}
p&=\left[\begin{array}{c}
-z \cos\left(\theta_{r}+\displaystyle\frac{\pi}{2}\right) \\
-z \sin\left(\theta_{r}+\displaystyle\frac{\pi}{2}\right)
\end{array}\right] \\
\\
&=\left[\begin{array}{c}
z \sin\theta_{r} \\
-z \cos\theta_{r}
\end{array}\right]
\end{align}

上式を時間微分することでが得られます．

\begin{align}
\dot{p}&=\left[\begin{array}{c}
\dot{z} \sin\theta_{r} + z\dot{\theta_{r}}\cos\theta_{r} \\
-\dot{z} \cos\theta_{r} + z\dot{\theta_{r}}\sin\theta_{r}
\end{array}\right]
\end{align}

ここで，を連鎖律より以下の形式に変換します．

\begin{align}
\dot{\theta_{r}}&=\frac{d\theta_{r}}{dt}=\frac{d\theta_{r}}{ds_{r}}\frac{ds_{r}}{dt} \\
\\
&= \kappa_{r} \dot{s_{r}}
\end{align}

ただし，は参照軌道の曲率です．

上記の結果をまとめて書くと，以下の幾何学的な関係が得られます．

\begin{align}
\dot{q_{r}} &= \dot{q} + \dot{p} \\
\\
\left[\begin{array}{c}
\dot{s_{r}} \cos\theta_{r} \\
\dot{s_{r}} \sin\theta_{r}
\end{array}\right] &= \left[\begin{array}{c}
v \cos\theta \\
v \sin\theta
\end{array}\right] + \left[\begin{array}{c}
\dot{z} \sin\theta_{r} + z\kappa_{r}\dot{s_{r}}\cos\theta_{r} \\
-\dot{z} \cos\theta_{r} + z\kappa_{r}\dot{s_{r}}\sin\theta_{r}
\end{array}\right]
\end{align}

発見的仮定

• 状態を位置ベクトルと逆方向に取る．
• 時間微分を曲率と参照軌道上の速度に分解する．

システムを座標変換する

先ほど求めた関係式をとについてまとめます．

\begin{align}
\left[ \begin{array}{cc}
-\sin\theta_{r} & (1-z\kappa_{r})\cos\theta_{r} \\
\cos\theta_{r} & (1-z\kappa_{r})\sin\theta_{r}
\end{array} \right] \left[ \begin{array}{c}
\dot{z} \\
\dot{s_{r}}
\end{array} \right] = \left[ \begin{array}{c}
v\cos\theta \\
v\sin\theta
\end{array} \right]
\end{align}

上式をとについて解きます．

\begin{align}
\left[ \begin{array}{c}
\dot{z} \\
\dot{s_{r}}
\end{array} \right] &= -\frac{1}{(1-z\kappa_{r})} \left[ \begin{array}{cc}
(1-z\kappa_{r})\sin\theta_{r} & -(1-z\kappa_{r})\cos\theta_{r} \\
-\cos\theta_{r} & -\sin\theta_{r}
\end{array} \right] \left[ \begin{array}{c}
v\cos\theta \\
v\sin\theta
\end{array} \right] \\
\\
&= -\frac{1}{(1-z\kappa_{r})} \left[ \begin{array}{c}
v(1-z\kappa_{r}) (\sin\theta_{r}\cos\theta - \cos\theta_{r}\sin\theta) \\
-v(\cos\theta_{r}\cos\theta + \sin\theta_{r}\sin\theta)
\end{array} \right] \left[ \begin{array}{c}
v\cos\theta \\
v\sin\theta
\end{array} \right] \\
\\
&= \left[ \begin{array}{c}
-v\sin(\theta_{r}-\theta) \\
\displaystyle\frac{v}{1-z\kappa_{r}}\cos(\theta_{r}-\theta)
\end{array} \right]
\end{align}

計算の過程で逆行列の計算と，三角関数の加法定理を使っています．

この時間微分形式の状態方程式を座標変換していきます．
まずは，を求めます．

\begin{align}
\frac{dz}{ds} &= \frac{dz}{dt} \frac{dt}{ds} \\
\\
&= -v\sin(\theta_{r}-\theta) \cdot \frac{1}{v} \\
\\
&= -\sin(\theta_{r}-\theta)
\end{align}

次に2階微分を求めます．
まず連鎖律で変換して，

\begin{align}
\frac{d^2z}{ds^2} &= \frac{d}{ds}\left( \frac{dz}{ds} \right) \\
\\
&= \frac{d}{ds} \left( -\sin(\theta_{r}-\theta) \right)\\
\\
&= -\cos(\theta_{r}-\theta) \cdot \left( \frac{d\theta_{r}}{ds} -\frac{d\theta}{ds} \right) \\
\\
&= -\cos(\theta_{r}-\theta) \cdot \left( \frac{d\theta_{r}}{ds_{r}} \frac{ds_{r}}{dt} \frac{dt}{ds} - \frac{d\theta}{dt} \frac{dt}{ds} \right)
\end{align}

 これまでに求めた式を代入していきます．

\begin{align}
\frac{d^2z}{ds^2} &= -\cos(\theta_{r}-\theta) \cdot \left( \kappa_{r} \cdot \frac{v}{1-z\kappa_{r}}\cos(\theta_{r}-\theta) \cdot \frac{1}{v} - \frac{v}{l}\tan\delta \cdot \frac{1}{v} \right) \\
\\
&= \cos(\theta_{r}-\theta) \cdot \left( \frac{-\kappa_{r}}{1-z\kappa_{r}}\cos(\theta_{r}-\theta) + \frac{1}{l}\tan\delta \right)
\end{align}

これで以下のような状態方程式が得られました．

非線形状態方程式

\begin{align}
\frac{d}{ds} \left[\begin{array}{c}
z \\
\displaystyle\frac{dz}{ds}
\end{array}\right] = \left[\begin{array}{c}
\displaystyle\frac{dz}{ds} \\
\cos(\theta_{r}-\theta) \cdot \displaystyle\left( \frac{-\kappa_{r}}{1-z\kappa_{r}}\cos(\theta_{r}-\theta) + \frac{1}{l}\tan\delta \right)
\end{array}\right]
\end{align}

この式に対して直接，非線形モデル予測制御などの手法を適用することもできますが，
今回は入力変換によって下記のような線形の状態方程式に変換します．

線形状態方程式

\begin{align}
\frac{d}{ds} \left[ \begin{array}{c}
z \\
\displaystyle\frac{dz}{ds}
\end{array} \right] = \left[ \begin{array}{cc}
0 & 1 \\
0 & 0
\end{array} \right] \left[ \begin{array}{c}
z \\
\displaystyle\frac{dz}{ds}
\end{array} \right] + \left[ \begin{array}{c}
0 \\
1
\end{array} \right] \mu
\end{align}

線形化された式を非線形の式と比較すると，であることがわかります．

したがって，入力変換の式は以下となります．

\begin{align}
\mu &= \cos(\theta_{r}-\theta) \cdot \displaystyle\left( \frac{-\kappa_{r}}{1-z\kappa_{r}}\cos(\theta_{r}-\theta) + \frac{1}{l}\tan\delta \right) \\
\\
\frac{1}{l}\tan\delta &= \frac{\mu}{\cos(\theta_{r}-\theta)} + \frac{\kappa_{r}}{1-z\kappa_{r}}\cos(\theta_{r}-\theta) \\
\end{align}

入力変換

\begin{align}
\delta &= \tan^{-1} \left( \frac{\mu l}{\cos(\theta_{r}-\theta)} + \frac{\kappa_{r} l}{1-z\kappa_{r}}\cos(\theta_{r}-\theta) \right)
\end{align}

状態方程式に対してフィードバックをかけて得たを上式に代入することで，
目標とする車両操舵角を計算できます．

ただし，この入力変換の過程で参照軌道の接線方位，車両方位，参照軌道の曲率が
急激に変化しないことを仮定していることに注意が必要です．

シミュレーション

MATLAB/Simulink R2015aでシミュレーションしてみました．

といってもほとんどコードベースなので，mファイルを見れば何をしているかわかると思います．

実装に必要な情報

時間軸状態制御のコントローラ内部は線形状態方程式に対する状態フィードバックと，
入力変換の式の2つだけです．

よって必要な情報は，車両から参照経路までの距離とその微分，
参照軌道と車両の方位誤差，参照経路の曲率，ホイールベースです．

距離，方位誤差および曲率ですが，
複雑な経路を走行させる場合にはパラメトリック曲線を利用すると簡単に計算することができます．

今回はCatmull-Rom splineを利用し，ニュートン法で車両から参照経路までの距離を計算しています．

このとき同時にFrenet-Serretの公式より，経路の接線方向と曲率も計算しています．

あとは自車の方位を取得できれば実装が可能です．

またですが，「車両の進んだ長さあたりのの変化」であるため，
表記通りに計算すると停止時に分母がゼロとなりゼロ割の恐れがあります．

ただし，先述した以下の式を利用すると問題なさそうなので今回はこちらを使用しています．

\begin{align}
\frac{dz}{ds} = -\sin(\theta_{r}-\theta)
\end{align}

シミュレーション

灰色の線が追従すべき参照経路，赤色の点が車両を表しています．

赤色の円は車両から参照経路までの距離，青の直線は参照経路上の接線方向を描画したものです．

---

この結果はSimlink project形式でGithubにアップロードしています．

github.com