Optunaで制御系のパラメータ調整

#概要
株式会社Preferred NetworksのOptunaは，詳しく知りませんがベイズ最適化の一種のTPEを使用して，評価関数$J(x)$があった時にarg min $J(x)$を探索してくれるみたいです。これを使用して決められた指令値に追従するための制御系のパラメータ調整をさせてみました。指令値形状および状態量数，制御系次数など制御器構造の最適化は扱っていません。
##問題設定
下図のような非線形性を有する3慣性系に位置，速度，捻れ反力を使用した制御器を構成することを試みます。慣性間の復元力定数が質点位置に依存するようにしてみました。
![3mass.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/43995/dcb2fd91-39fb-f51a-e24a-a62c7355e16f.png)
今回は位置指令を単位ステップ状に変化させることにします。この系に対して位置制御系を組んだ場合には負荷位置応答は発振します。
![conv.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/43995/b32078be-8fa7-3008-b47b-3d8637823c88.png)
三慣性系であるにも関わらず位置応答が三角波状となり、基本波と奇数倍の高調波が多く含まれることが確認できました。これは導入した非線形性に起因すると予想されますが、系の特性が把握しづらくなっています。

##パラメータ探索させてみた
Optunaはpythonから使用しています。製造業では機械制御用の計算機(Programmable Lgic Controller: PLC)が設けられており、これと通信して機械の運転とパラメータ探索を進めます。今回はPLCエミュレータとプラントシミュレータをc++で実装し，ソケット通信を用いて探索器と制御器を繋げました。

``` Optuna.py
import optuna
import socket
import time
import struct
import binascii

def objective(trial):
	# Parameter desgin by the optimizer.
	x = trial.suggest_uniform('x', 1000, 12000)
	y = trial.suggest_uniform('y', 200, 400)
	z = trial.suggest_uniform('z', 0, 30)

	try:
		#Send parameters
		s.sendall(str(x).encode())
		time.sleep(0.1)
		s.sendall(str(y).encode())
		time.sleep(0.1)
		s.sendall(str(z).encode())
		time.sleep(0.1)
		#Receive score
		score=float(s.recv(4096).decode('utf-8').replace('\0',''))
	except socket.timeout:
		s.close()
		print('Failed to write data on R registers.')

	# Return the score to the optimizer.
	return score

s=socket.socket(socket.AF_INET, socket.SOCK_STREAM)
s.settimeout(3.0)
print('Connecting to the PLC...')
try:
    s.connect(('127.0.0.1', 0x2000))
except socket.error:
    s.close()
    print('Failed to connect to the PLC.')

print('This PC is correctly connect to the PLC.')
print('Starting communication:')
time.sleep(0.5)

#plant = Plant()
study = optuna.create_study()
study.optimize(objective, n_trials=100)

data_endcode=b"end"
s.sendall(data_endcode)

print("\n")
print("Parameters:", study.best_params)
print("Value:", study.best_value)
print("\n")

s.close()
```
以下はソケット通信をするPLCエミュレータです

```PLC.cc
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <fcntl.h>
#include <unistd.h>
#include <arpa/inet.h>

#include "motion.hh"

int main(){

  int sock0;
  struct sockaddr_in addr;
  struct sockaddr_in client;
  unsigned int len;

  sock0 = socket(AF_INET, SOCK_STREAM, 0);

  addr.sin_family = AF_INET;
  addr.sin_port = htons(0x2000);
  addr.sin_addr.s_addr = INADDR_ANY;
  bind(sock0, (struct sockaddr *)&addr, sizeof(addr));
  listen(sock0, 5);

  char buf_send[100];
  char buf_recv[100];
  memset(buf_send,0,sizeof(buf_send));
  memset(buf_recv,0,sizeof(buf_recv));

  const int param_num=3;
  double param[param_num]={0.0};
  double score=0.0;

  double param_best[param_num]={0.0};
  double score_best=1.7e+308;

  int flag_trial=0;
  int cnt=0;
  const int disable=0;
  const int enable=1;

  len = sizeof(client);
  int sock = accept(sock0, (struct sockaddr *)&client, &len);

  while(flag_trial==0){
    for(int i=0;i<param_num;i++){
      memset(buf_recv,0,sizeof(buf_recv));
      recv(sock, buf_recv, sizeof(buf_recv), 0);
      if(strcmp(buf_recv,"end")==0) flag_trial=1;
      param[i]=atof(buf_recv);
    }
    if(flag_trial==0){
      cnt++;
      score=motion_sim(param[0],param[1],param[2]);
      printf("No. %d: parameters=%lf, %lf, %lf, score=%lf\n", cnt, param[0], param[1], param[2], score);

      sprintf(buf_send, "%lf", score);
      send(sock, buf_send, sizeof(buf_send), 0);

      if(score<score_best){
        score_best=score;
        for(int k=0;k<param_num;k++) param_best[k]=param[k];
      }
    }
  }

  score_best=motion_sim(param_best[0],param_best[1],param_best[2]);
  printf("Best parameters=%lf, %lf, %lf, Best score=%lf\n", param_best[0], param_best[1], param_best[2], score_best);

  close(sock0);

  return 0;
}
```
以下はプラントシミュレータです。スコアもこの中で算出しています。

```motion.hh
#include <stdio.h>
#include <math.h>

double motion_sim(double Kp, double Kd, double Kf);
```

```motion.cc

#include <motion.hh>

double motion_sim(double Kp, double Kd, double Kf){

    double t=0.0;

    const double Tsim=5.0;
    const double Ts=1e-4;
    const int sloop=(int) (Tsim/Ts);
    const int ploop=100;
    const double Tp=1e-4/(double) ploop;

    double score=0.0;

    //Motor side
    double xm_cmd=0.0;
    double xm_res=0.0;

    const double Kt=1.3;
    const double Mm=0.3;

    double im_ref=0.0;
    double ddxm_ref=0.0;
    double ddxm_res=0.0;
    double dxm_res=0.0;
    double fm_drv=0.0;

    //env1ironment side
    double xenv1_res=0.0;
    double dxenv1_res=0.0;
    double ddxenv1_res=0.0;
    double fenv1=0.0;
    const double env1_k0=23456.7;
    double env1_k=0.0;
    const double env1_d=0.1;
    const double env1_m=5.4;

    double xenv2_res=0.0;
    double dxenv2_res=0.0;
    double ddxenv2_res=0.0;
    double fenv2=0.0;
    const double env2_k0=34567.8;
    double env2_k=0.0;
    const double env2_d=0.1;
    const double env2_m=3.4;

    for(int i=0; i<sloop; i++){

      xm_cmd=(t>1.0)? 1.0:0.0;
      ddxm_ref=Kp*(xm_cmd-xm_res)-Kd*dxm_res + fenv1*Kf;
      im_ref=(Mm/Kt)*ddxm_ref -fenv1/Kt;

      for(int j=0; j<ploop; j++){
        env1_k= env1_k0*exp(-fabs(xenv1_res-xm_res));
        env2_k= env2_k0*exp(-fabs(xenv2_res-xenv1_res));
        fm_drv=Kt*im_ref;
        fenv2=env2_k*(xenv2_res-xenv1_res)+env2_d*(dxenv2_res-dxenv1_res);
        fenv1=env1_k*(xenv1_res-xm_res)+env1_d*(dxenv1_res-dxm_res);
        xm_res+=dxm_res*Tp;
        xenv1_res+=dxenv1_res*Tp;
        xenv2_res+=dxenv2_res*Tp;
        dxm_res+=ddxm_res*Tp;
        dxenv1_res+=ddxenv1_res*Tp;
        dxenv2_res+=ddxenv2_res*Tp;
        ddxm_res=(fm_drv+fenv1)/Mm;
        ddxenv1_res=(-fenv1+fenv2)/env1_m;
        ddxenv2_res=(-fenv2)/env2_m;
        t+=Tp;
      }

      if(xm_cmd-xenv2_res<0.0) score+=50.0*pow(xm_cmd-xenv2_res,2.0)*Ts;
      else score+=pow(xm_cmd-xenv2_res,2.0)*Ts;
    }

    score=(score>1e+300)? 1e+300:score;
    return score;
}

```
オーバーシュートの発生を抑えるため、スコアの算出においてオーバーシュートの損失を大きく設定してみました。100回ほどの探索で見つけてもらったパラメータでプラントシミュレータを回すと，いい感じの応答がでました。
![prop.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/43995/a4203cd6-9989-e510-7268-1d5a1ddba0d0.png)
オーバーシュートは残りましたが、慣性間距離が減少すると柔らかくなるバネで繋がれていると考えると仕方ないかもしれません。

また，復元力定数は，上記2つのシミュレーションで以下のように変化していました。
![stiff.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/43995/2848a33b-e074-45e7-c422-8f530d49c232.png)
非線形プラントに対して人間がパラメータ調整するのは大変そうですが，Optunaは特にプラントの情報を与えることなく良い感じのパラメータを見つけてくれました。振動は駆動側と負荷側のインピーダンスを整えれば抑えられることが知られており、このような経験知から人間はある程度のパラメータ調整ができますが，手間がかかるのでこの程度のことは計算機に任せられるようにしたいですね。

逐一システム同定しても良いとは思いますが、測定機材の導入と適切な設置と使用、システム同定における入力とアルゴリズムの選定、試験に必要な知識を持った人員と工数を考えると実施は非常に難しいと思われます。大規模機械となれば機械自体が理想的に動作していたとしても同定は難しく、今回の例のように非線形性が含まれる場合には制御系の設計まで含めると徒労に終わることが予想されます。製造業で使用する汎用PLCは単純な制御系しか組めないので、最初からパラメータ調整をしても良いのではないでしょうか。