westoshy
9/4/2016 - 5:38 AM

## 制御シミュレーション手法

``````#pragma once

#include <iostream>
#include <Eigen/Dense>

// Equation of Motion
template <class T> class EoM
{
private:
Eigen::MatrixXd A_;
Eigen::MatrixXd B_;
Eigen::VectorXd u_;

public:
// Matrix A, B and initial controll input u
EoM(const Eigen::MatrixXd &A, const Eigen::MatrixXd &B,
const Eigen::VectorXd &u) :
A_(A), B_(B), u_(u)
{
}

// Controll input
void inputControll(const Eigen::VectorXd &u)
{
u_ = u;
}

// dx/dt = f(x, t)
Eigen::MatrixXd operator() (const Eigen::VectorXd &x, T t) const
{
return A_ * x + B_ * u_;
}
};``````
``````#include <iostream>
#include <fstream>
#include "integrator.hpp"
#include "plant.hpp"

int main(void)
{
Eigen::MatrixXd A(2,2);
Eigen::MatrixXd B(2,1);
Eigen::VectorXd x(2), dx(2);
Eigen::VectorXd u(1);

// System Parameter
const double D  = 0.01;
const double M  = 0.30;
const double Kt = 22.0;

// Parameters
A << 0, 1, 0, -D/M;
B << 0, Kt/M;
x << 0, 0;
dx << 0,0;
u << 0;
std::cout << "A, B" << std::endl;
std::cout << A << std::endl;
std::cout << B << std::endl;

std::cout << "controll input" << std::endl;
std::cout << B * u << std::endl;

EoM<double> eom(A, B, u);

std::ofstream ofs("out.dat");

const double dt = 0.01;

const double dmp = 1.0;
const double Kp = 100.0;
const double Kd = 2.0 * sqrt(Kp) * dmp;
const double Mn = 0.3;
const double Ktn = 22.0;
Eigen::VectorXd x_cmd(2);
x_cmd << 0.0, 0.0;

for(double t=0.0; t<10.0; t+=dt) {
if( t>=1.00 ) {
x_cmd << 1.0, 0.0;
}
u[0] = (Mn / Ktn) * (Kp * (x[0] - x_cmd[0]) + Kd * (x[1] - x_cmd[1]) );
// cout << u << endl;
eom.inputControll(u);
x = Euler1st(eom, x, t, dt);
ofs << t << "," << x[0] << "," << x[1] << std::endl;
}

}``````
``````#pragma once

// 1st Euler Method
template <class Function, class V1, class V2>
V1 Euler1st(Function f, V1 x, V2 t, V2 delta){
return x + f(x, t) * delta;
}

// 2nd Euler Method
template <class Function, class V1, class V2>
V1 Euler2nd(Function f, V1 x, V2 t, V2 delta){
V1 k1 = f(x, t) * delta;
V1 k2 = f(x + k1, t + delta) * delta;
return x + (k1 + k2)/2;
}

// Runge Kutta Method
template <class Function, class V1, class V2>
V1 RungeKutta4th(Function f, V1 x, V2 t, V2 delta){
V1 k1 = f(x, t) * delta;
V1 k2 = f(x + k1/2, t + delta/2) * delta;
V1 k3 = f(x + k2/2, t + delta/2) * delta;
V1 k4 = f(x + k3, t + delta) * delta;
return x + (k1 + k2*2 + k3*2 + k4)/6;
}``````

プロジェクトファイル