最適化函数を使って船を横移動させてみる

この記事は制御工学アドベントカレンダー9日目の記事です．
さいきん，仕事の方で船に関わることが多いので船の動きも面白いよ，と布教してみる．
船，とひとくちに言ってもタンカーなどの超大型船から客船，個人所有のクルーザー，漁船までいろいろ種類があります．
また，推進方法によっても船内にエンジンを積んだ船内機艇，エンジンごと取り外し可能な船外機艇，ジェット推進の艇などの種類があります．

ここではクルーザーなどに多く使われる下図のような船外機艇(Outboard motor)に焦点を当ててみます．

Mercury社のウェブページ
図の船は船外機が2つ取り付けられています（2機掛けと呼びます）が，最大で5つ取り付ける方もいるようです．
250馬力を5機掛け．パワーの面でも価格の面でも恐ろしいですね．

---

その船外機艇のなかでも面白いなあと思ったものがMercury社のSkyhookというJoystick操船システムです．
1分34秒あたりを見るとわかりやすいですが，着岸状態から船がほぼ真横に移動しています．

Mercury Joystick Piloting for Outboards
通常，船外機は何機掛けであろうと同じ方向を向かせて舵を切るのですが，この操船システムではそれぞれの船外機を独立させて反対方向に動かすことで横移動を実現しています．

このSkyhookを再現するため，最適化函数を使って遊んでみようというのがこの記事の趣旨です．
環境はMATLAB/Simulink R2015aです．

---

さて，基本的な船の運動形態ですが，前進・後退・旋回の3つがあります．
動力もステアリングも重心後方にあるので，前輪駆動の車でバック走行している状態に近いのではないかなと思います．

この3つを見て分かるとおり，船は基本的に真横には進みません．

船外機の動作角度に制限があり，真横方向に力を発生させられないためです．
（通常は-30deg~+30deg程度）
そこで先程の動画のように左右の船外機を独立に”開いて”動かすることを考えます．
このとき，4つめの図のように推進力（スラスト; Thrust）が左右の船外機で同方向だと効率を落として前に進むだけですが，左右で逆向きのスラストを発生させることで横向きの力が発生します．

前後方向の力$X$，左右方向の力$Y$，回転方向のモーメント$N$は以下の式で表すことができます．
$$X = T_{1}\cos\alpha_{1} + T_{2}\cos\alpha_{2} \\ Y = T_{1}\sin\alpha_{1} + T_{2}\sin\alpha_{2} \\ N = r_{1x}T_{1}\sin\alpha_{1} - r_{1y}T_{1}\cos\alpha_{1} + r_{2x}T_{2}\sin\alpha_{2} - r_{2y}T_{2}\cos\alpha_{2}$$
ただし$T_{1}$(N)，$\alpha_{1}$(rad)，$r_{1x}$(m)，$r_{1y}$(m)はそれぞれ船外機1のスラスト，ステアリング角，重心から船外機回転軸までの$x$方向距離，$y$方向距離です．

---

Skyhookでは左右の船外機は同じだけ反対方向に動いているらしく，これを基に上の式で$\alpha=\alpha_{1}=-\alpha_{2}$とすると以下の式が得られます．
$$X = ( T_{1} + T_{2} )\cos\alpha \\ Y = ( T_{1} - T_{2} )\sin\alpha \\ N = ( T_{1} - T_{2} )( r_{x}\sin\alpha - r_{y}\cos\alpha )$$
また，簡単化のため$r_{x}=r_{1x}=r_{2x}$，$r_{y}=r_{1y}=-r_{2y}$としています．（x軸に対して船外機が線対称に配置）
さて，上で求めた式に同じ大きさの逆向きのスラストを与えてみる，つまり$T=T_{1}=-T_{2}$とすると，
$$X = 0 \\ Y = 2T\sin\alpha \\ N = 2T( r_{x}\sin\alpha - r_{y}\cos\alpha )$$
となります．前後方向の力が消えましたが，ヨー方向のモーメントが残っています．
船が真横に移動するためには，ヨー方向のモーメント$N=0$となる必要があります．
これは解析的に解け，以下のようにヨー方向のモーメントをゼロにするようなステアリング角$\alpha^{*}$が得られます．
$$r_{x}\sin\alpha - r_{y}\cos\alpha = 0 \\ \alpha^{*} = tan^{-1}\left(\frac{r_{y}}{r_{x}}\right)$$
このステアリング角を維持することで，船体の真横方向への移動が可能になります．

車では摩擦によって車輪が横滑りしないため横方向の力を発生でも車体が横方向に動くことはありませんが，船の場合は横方向の抗力が前後方向に比べて非常に大きいというだけですので，ゆっくりとであれば真横への移動も可能なのです．

---

実際にはSkyhookでは前後移動，左右移動，回転移動の他に，前後，左右，回転を組み合わせた移動も行うことができます．
どういうことかというとジョイスティック型のコントローラを使用しているため，姿勢を変えずに斜め前方への移動なども可能なのです．ジョイスティックを捻ることでその場での回転や斜めに移動しながらの回転なども可能です．

これらを同時に行おうとした場合，以下のような計算を行う必要があります．
$$X_{Desired} = ( T_{1} + T_{2} )\cos\alpha \\ Y_{Desired} = ( T_{1} - T_{2} )\sin\alpha \\ N_{Desired} = ( T_{1} - T_{2} )( r_{x}\sin\alpha - r_{y}\cos\alpha )$$
ただし，$X_{Desired}$，$Y_{Desired}$，$N_{Desired}$はそれぞれ目標とする$x$方向の力，$y$方向の力，$\psi$方向（ヨー方向）のモーメントです．

目標とする力とモーメントを与えようとしたとき，もはや解析的に解くことは困難です．
最適化の出番です．

---

種々の最適化を行おうとするとき，最もよく利用されるのが誤差を2次の形式で表して最小化する方法です．
$$[T_{1}^{*}, T_{2}^{*}, \alpha^{*}] = \min_{T_{1}, T_{2}, \alpha}( J ) \\ J = \left( X_{Desired} - ( T_{1} + T_{2} )\cos\alpha \right)^2 + \left( Y_{Desired} - ( T_{1} - T_{2} )\sin\alpha \right)^2 \\ + \left( N_{Desired} - ( T_{1} - T_{2} )( r_{x}\sin\alpha - r_{y}\cos\alpha ) \right)^2$$
この式は，それぞれの力，モーメントの誤差を2乗して足した函数$J$が最小となるような$T_{1}^{*}$，$T_{2}^{*}$，$\alpha^{*}$を探索するという意味を持ちます．
$J$は評価函数，価値函数，コスト函数などと呼ばれます．
それぞれの誤差を2乗にすることで，誤差が負になった際にも函数の値が大きくなるようになっています．
この誤差を最小化するということは“目標値”と”操作した結果の出力値”の差が小さくなるように操作量を決めようという意味になります．

この最小化問題を解くための方法にはいろいろありますが，今回はあまり深入りせずMATLABで標準函数として提供されているfminsearch（Nelder-Mead法）を使ってみます．
fminsearchの実装はMathWorksのウェブページに掲載されています．

---

fminsearchでは最小化したい函数のハンドラ@ObjectiveFunctionと，変数の初期値x_initialを与えます．

x = fminsearch( @ObjectiveFunction, x_initial );

今回，最適化変数は左右のスラスト$T_{1}$，$T_{2}$とステアリング角$\alpha$の3つなので，x_initialは3次元のベクトルとして与えます．

x_initial = [ 100; -100; 0.1; ];

ObjectiveFunctionは先ほどの評価関数を基に，次のように定義します．

function ret = ObjectiveFunction( x0 )
    T1 = x0(1);
    T2 = x0(2);
    SteerAngle = x0(3);

    X_T1 = T1 * cos( SteerAngle );
    Y_T1 = T1 * sin( SteerAngle );
    X_T2 = T2 * cos( -SteerAngle );
    Y_T2 = T2 * sin( -SteerAngle );

    X = X_T1 + X_T2;
    Y = Y_T1 + Y_T2;
    N = ( OM1_rb2om_X * Y_T1 - OM1_rb2om_Y * X_T1 ) + ( OM2_rb2om_X * Y_T2 - OM2_rb2om_Y * X_T2 );

    ret = ( Desired_X - X )^2 + ( Desired_Y - Y )^2 + ( Desired_N - N )^2;
end

あとはこれをMATLAB Functionを利用してSimulink内に実装するだけです．

---

ここからSimulink内での細かい話になるので，気になる方以外は読み飛ばしてもらえればと思います．
Simulinkにfminsearchを実装する際に問題となるのが，ObjectiveFunctionに引数を取れないという点です．函数は最適化変数（ここではx0）のみを持つ必要があります．
このため，目標の力 / モーメントを評価函数に入れることができません．
そこで今回はglobal変数を利用して無理やりfminsearchに引数を渡しています．
ここがSimulinkの弱いところで，実時間の最適化をやろうとする場合は最適化函数の殆どが使えません．

fminsearchはかろうじてMATLAB Function経由で呼ぶことができますが，fmincon（制約付き最適化の函数）などに至っては呼ぶことすらできません．
MathWorksの方に聞いたところ，今のところSimulinkから呼ぶような用途は想定していないとのことでした．
似たようなことをする場合，運動のモデルごとMATLAB側に書いたほうが良さそうです．

具体的には，以下のようにData Store Memoryを利用してMATLAB Functionからglobal変数として読み出しています．詳細はコードをみればわかるかと思います．

---

さて，船のシミュレーションをするためにここで運動モデルを立てます．
Fossenのテキストがよくまとまっているので，こちらを利用します．

• Amazon
• Fossen氏のウェブページ（テキストをまとめたスライドが無料公開されています）

テキストより，船の重心周りの運動方程式は速度，角速度ベクトルを$\nu=[ u, v, w, p, q, r ]^T$として以下のように得られます．
$$M_{RB}\dot{\nu} + C_{RB}(\nu)\nu + D\nu|\nu| = \tau_{RB}$$
ただし，$M_{RB}$は慣性マトリクス，$C_{RB}$は遠心力 / コリオリ力の項，$D$は粘性マトリクス，$\tau_{RB}$はスラストなどの外力です．
今回は波などの外乱は無視できるものとしています．
また，船の重心周りに作用するスラスト$\tau_{RB}=[ X, Y, Z, L, M, N ]^T$は以下で表すことができます．
$$\tau_{RB} = \tau_{1} + \tau_{2} = \left[\begin{array}{c} R_{1} \cdot TH_{1} \\ r_{1OM/g} \times ( R_{1}\cdot TH_{1} ) \end{array}\right] + \left[\begin{array}{c} R_{2} \cdot TH_{2} \\ r_{2OM/g} \times ( R_{2}\cdot TH_{2} ) \end{array}\right]$$
ただし，$R_{j}$は$j$番目の船外機座標系から船体の座標系への回転マトリクス，$TH_{j} = [ T_{j}, 0, 0 ]^T$は$j$番目の船外機が発生する船外機に固定した座標系でのスラストベクトル，$r_{jOM/g}$は船体の重心から$j$番目の船外機までの位置ベクトルです．

クロス積を利用すると，重心から力の作用点までのベクトルと作用している方向から，重心周りの並進力とモーメントが簡単に計算できるので便利ですね．このあたりは長くなりそうなので省略しますが．

---

さて，これらをMATLAB/Simulink R2015aで実装してみた結果が下の図です．
横方向，前後方向，回転方向そして斜め方向へ移動できていることが確認できます．
今回フィードバックは入れていませんが，MercuryのウェブページにはGPSと連携云々と書かれているので実際には位置や方位のフィードバックは入っていそうです．

ソースファイル（Simulink project形式）はGithubにアップロードされています．
前回に引き続きGithubの使い方が理解できていませんがご容赦を．
BoadSim_Skyhook

---

さいごに．
このような面白い動作ができるのは船ならではだと思います．
他の面白い事象として，回転半径が速度に依存する現象や，同じだけステアリングを切り返しているのに左右対称の動きとならない現象などもあります．

上の図ではずっと同じだけ舵を切っていますが，速度が上昇するにつれて回転半径が小さくなっています．
このため，4輪の制御でよく用いられている車輌とステアリングの幾何学的な関係だけでは上手く制御できなかったりするようです．

こちらは左に舵を切ったあと，同じだけ右に舵を切っていますが，航跡（軌跡）が左右対称になっていません．車でいうとドリフト走行しているような状態になります．
さらに今回は粘性力（抗力）も船体の重心に作用するものとしてモデル化しているため最後に右側を向いていますが，実際には時々刻々と抗力の作用点も変わっていくため，ステアリングを同じだけ切っても図のように最初と同じ方向には向きません．
このあたりもなかなか面白いなあと感じています．

船の制御，いかがでしょうか？

---

参考

• Mathworks, “非線形関数の最適化”, https://jp.mathworks.com/help/matlab/math/optimizing-nonlinear-functions.html#bsgpq6p-11.
• Thor I. Fossen, “Handbook of Marine Craft Hydrodynamics and Motion Control”, amazon, 著者Webページ.

わがふるさと琵琶湖でクルーレス・ソーラーボート大会なるものも開催されているようです．
機会があればこういうのにも出てみたいなあ．