2020-08-16

ニュートン法

ニュートン法（ニュートン・ラフソン法）は、方程式を数値計算によって解くための反復的な方法による求根アルゴリズムの１つです。

ja.wikipedia.org

f:id:Manao55:20200816204520j:plain

①根の近くの値 $x_0$ を初期値にします。

② $y=f(x)$ の $x=x_0$ における接戦を引き、 $x$ 軸と交わったところを $x_1$ とし、以下同様の手順で $x_2, x_3 , ... , x_{n}$ と求めます。

③ $x_n$ を求めるには、

${ \displaystyle f'(x_{n-1})≃\frac{f(x_{n-1})}{x_{n-1}-x_n} }$

という性質があることを用いて、

${ \displaystyle x_n = x_{n-1}-\frac{f(x_{n-1})}{f'(x_{n-1})} }$

と、前の値 $x_{n-1}$ を用いて求めることができます。

${ \displaystyle \frac{|x_n-x_{n-1}|}{|x_{n-1}|} < EPS }$

になったときの $x_n$ の値を求める根とし、そうでないなら②を繰り返します。 $EPS$ は適当な精度を選びます。

プログラム

#include <iostream>
#include <math.h>
using namespace std;

#define f(x)  ((x)*(x)-2)
#define g(x)  (2*(x))
#define EPS   1e-09
#define LIMIT 50

int main(void)
{
    double x=2.0,dx;
    int k;

    for(k=1;k<=LIMIT;k++){
        dx = x;
        x=x-f(x)/g(x);
        if (fabs(x-dx)<fabs(dx)*EPS){
            cout << "x=" << x << endl;
            break;
        }
    }
    if (k>LIMIT)
        cout << "収束しない" << endl;
}

実行結果

x=1.41421

2020-08-14

数値積分

台形則による定積分

関数 $f(x)$ の定積分 $\int_{a}^{b}f(x)dx$ を台形則により求めてみます。

関数 $f(x)$ の定積分を微小区間に分割して近似値として求める方法を数値積分といいます。

f:id:Manao55:20200814022308j:plain

図に示すように, $a$ , $b$ 区間を $m$ 個の台形に分割し, 各台形の面積を合計すると,

${ \displaystyle \int_{a}^{b}f(x)dx = \frac{h}{2}(f(a)+f(a+h)) + \frac{h}{2}(f(a+h)+f(a+2h)) }$

${ \displaystyle + \frac{h}{2}(f(a+2h)+f(a+3h)) + ... + \frac{h}{2}(f(a+(n-1)h)+f(b)) }$

${ \displaystyle = h{\frac{1}{2}(f(a)+f(b))+f(a+h)+f(a+2h)+...+f(a+(n-1)h)} }$

となります。

プログラム

#include <iostream>
#include <math.h>
using namespace std;

#define f(x) (sqrt(4-(x)*(x))) /* 被積分関数 */

int main(void){
    int k;
    double a,b,n,h,x,s,sum;

    cout << "Integration interval A,B ? " << endl;
    cin >> a >> b ;

    n = 1000;
    h=(b-a)/n;
    x=a;
    s=0;
    for(k=1;k<=n-1;k++){
        x=x+h;
        s=s+f(x);
    }
    sum = h*((f(a)+f(b))/2+s);
    cout << "    /" << b << endl;
    cout << "    | sqrt(4-x*x) dx = " << sum << endl;
    cout << "    /" << a << endl;

    return 0;
}

実行結果

Integration interval A,B ?
0 2
    /2
    | sqrt(4-x*x) dx = 3.14156
    /0

2020-08-14

テイラー展開② - 三角関数

$cos{x}$ をテイラー展開してみる

$cos {x}$ をテイラー展開すると以下のようになります。

${ \displaystyle cos{x} = 1 - \frac{x^2}{2!} + \frac{x^4}{4!} - \frac{x^6}{6!} + ... }$

x の値は $0$ ~ $2\pi$ の範囲に収まるように計算します。

プログラム

一例（C++）を示します。

#include <iostream>
#include <iomanip>
#include <math.h>
using namespace std;

double tailercos(double);

int main()
{
    double x,rd=3.14159/180;

    cout << setw(5) << "x" << setw(14) << "tailercos(x)"  <<  setw(14) << "exp(x)" << endl;
    
    for (x=0;x<=180;x=x+10){
        cout << setw(5) << x << setw(14) << tailercos(x) << setw(14) << exp(x) << endl;
    }
    
    return 0;
}
double tailercos(double x){
    double EPS=1e-09;
    double s=1.0,e=1.0,d=1.0;
    int k;

    x = fmod(x,2*3.14159265358979);
    for(k=1;k<=200;k=k+2){
        d=s;
        e=-e*x*x/(k*(k+1));
        s=s+e;
        if(fabs(s-d)<EPS*fabs(d))
            return (s);
    }
    return (9999.0);
}

実行結果

    x  tailercos(x)        cos(x)
  0.0             1             1
 10.0      0.984808      0.984808
 20.0      0.939693      0.939693
 30.0      0.866026      0.866026
 40.0      0.766045      0.766045
 50.0      0.642788      0.642788
 60.0      0.500001      0.500001
 70.0      0.342021      0.342021
 80.0      0.173649      0.173649
 90.0  1.32679e-006  1.32679e-006
100.0     -0.173647     -0.173647
110.0     -0.342019     -0.342019
120.0     -0.499998     -0.499998
130.0     -0.642786     -0.642786
140.0     -0.766043     -0.766043
150.0     -0.866024     -0.866024
160.0     -0.939692     -0.939692
170.0     -0.984807     -0.984807
180.0            -1            -1

2020-08-12

アルゴリズム関係

manao55.hatenablog.com

2020-08-12

テイラー展開① - 指数関数

テイラー展開

数値解析などのアルゴリズムを勉強したいと思いましたので何か書いていきます。今回はテイラー展開について。

$e^{x}$ をテイラー展開してみる

$e^{x}$ をテイラー展開すると以下のようになります。

${ \displaystyle e^{x} = 1 + \frac{x}{1!} + \frac{x^2}{2!} + \frac{x^3}{3!} + ... + \frac{x^{k-1}}{(k-1)!} + \frac{x^{k}}{k!} + ... }$

この式は無限級数で展開しているため, 実際にプログラムに落とし込むためには有限回で打ち切る必要があります。条件は, $k-1$ 項までの和を $d$ , $k$ 項までの和を $s$ としたとき,

${ \displaystyle \frac{|s-d|}{|d|} < EPS }$

となります。 $|s-d|$ を打ち切り誤差, $\frac{|s-d|}{|d|}$ を相対打ち切り誤差というそうです。

今回は $EPS=1e-9$ とします。

プログラム

一例を示します。(C++)

#include <iostream>
#include <math.h>
#include <iomanip>
using namespace std;

double tailerexp(double);

int main(void){
    double x;
    cout << setw(5) << "x" << setw(14) << "tailerexp(x)"  <<  setw(14) << "exp(x)" << endl;
    
    for(x=0;x<=40;x=x+10){
        cout << setw(5) << x << setw(14) << tailerexp(x) << setw(14) << exp(x) << endl;
    }

    return 0;
}

double tailerexp(double x){
    double EPS=1e-09;
    double s=1.0,e=1.0,d=1.0;
    int k;

    for(k=1;k<=200;k++){
        d=s;
        e=e*x/k;
        s=s+e;
        if (fabs(s-d)<EPS*fabs(d))
        return(s);
    }

    return (0.0);
}

実行結果

    x  tailerexp(x)        exp(x)
    0             1             1
   10       22026.5       22026.5
   20   4.85165e+08   4.85165e+08
   30   1.06865e+13   1.06865e+13
   40   2.35385e+17   2.35385e+17

2020-07-05

単軸ロボットアームのサーボ制御設計 - 外乱オブザーバ

外乱

以前DCモータのサーボ制御（位置制御）について少しやっていました。 qiita.com

位置制御系では, 制御プラントが積分器を有しているため, I制御なくとも指令に追従しました。しかし外乱に対しては有効ではなく, 偏差を生じます。

"外乱 - 通信系などに外から加わる不要な信号。雑音あるいは妨害ともいう。"

今回は制御系におけるものですね。以下の図に制御入力に定常的な外乱（電圧オフセット）を加えた時の応答を示します。 f:id:Manao55:20200703160904j:plain このように, 偏差を確認することができます。

外乱オブザーバ

外乱オブザーバは, フィードバックループ内に外乱が発生する状況で, 外乱を推定して制御入力にフィードバックをかけることで, 比較的に簡易に外乱を除去し安定化することができます。補償器のひとつです。

arduinopid.web.fc2.com

qiita.com

今回は作ったサーボ系で動作を確かめてみようと思います。

外乱オブザーバの原理

今回の制御ループで外乱を有するときのブロック図を以下に示します。 f:id:Manao55:20200703182449j:plain このときの出力 $y$ は,

${ \displaystyle y = (u+d) G(s) }$

となります。コントローラによって制御された入力が外乱によって変化してしまいます。

上の例では, $d$ に一定値を入力しているので, オフセットの電圧がモータドライバに印加され, 定常偏差が発生します。

コントローラ側から生成される制御量から外乱を推定して, 制御入力に外乱 $d$ を差し引いておくことで外乱の影響を除去することができます。 f:id:Manao55:20200703195050j:plain

この場合の出力 $y$ は,

${ \displaystyle y = (u+d-d) G(s) \\ }$

${ \displaystyle = u G(s) }$

となり, 適切に制御されます。(外乱があらかじめわかっているのなら, フィードフォワードで差し引けば外乱を除去できます。)

本質はどう外乱を推定するかにあります。

出力は外乱を受けた後の出力となるため, ここから推定することができそうです。出力 $y$ の式は,

${ \displaystyle y = (u+d) G(s) }$

ここで制御プラント $G(s)$ が逆システム $G(s)^{-1}$ が存在すれば,

( 今回は $G(s)^{-1} = {s(sT+1)}/{T}$ となります )

${ \displaystyle y G(s)^{-1} = u+d }$

${ \displaystyle \hat{d} = y G(s)^{-1} - u }$

となり, 推定外乱 $\hat{d}$ を求めることができます。

ブロック図で表すと以下のように表現することができます。 f:id:Manao55:20200703211928j:plain

オレンジ部分がオブザーバの表現になります。コントローラによって生成される制御入力は, 推定がイランと外乱を受けて $u-\hat{d}+d$ となるので, 出力 $y$ は,

${ \displaystyle y = (u-\hat{d}+d) G(s) }$

となり, 推定外乱 $\hat{d}$ は,

${ \displaystyle \hat{d} = y G(s)^{-1} - (u - \hat{d}) }$

${ \displaystyle = (u-\hat{d}+d) G(s) G(s)^{-1} - (u - \hat{d}) }$

ここで $G(s)$ とその逆システム $G(s)^{-1}$ の積 $G(s)G(s)^{-1} = 1$ とすると,

${ \displaystyle \hat{d} = d }$

が成立します。このような原理で外乱を除去します。

プログラム上で制御対象の伝達関数 $G(s)$ , またその逆関数 $G(s)^{-1}$ を利用して制御する方式をモデル規範型制御とも呼んだりするそうです。

外乱オブザーバの実用上の運用

今回の制御プラントは逆システム $G(s)^{-1}$ が得られます。運用上で注意すべきことがあり, フィルタの設計になります。逆システムの式、 $G(s)^{-1}$ 　には微分項が含まれています。

微分動作は基本的に不安定な要素のため, これを避けるためにプロパーな関数にします。具体的な操作としては分子多項式の次数が分母多項式の次数以下にするためローパスフィルタを使います。

今回私は, Butterworthフィルタを選定しました。

${ \displaystyle F(s) = \frac{\omega_c^2}{s^2+\sqrt{2}\omega_c s + \omega_c^2} }$

カットオフ周波数 $\omega_c$ となります。

推定外乱にフィルタをかけることも多いそうですが, ループ内に微分器が残ってしまうパスが残ってしまうので, ローパスフィルタを分散して次のように設定します。 f:id:Manao55:20200704021534j:plain

こうすることで出力項は以下のように変形され, プロパーな項となります。

${ \displaystyle F(s) G(s)^{-1} = \frac{\omega_c^2}{s^2+\sqrt{2}\omega_c s + \omega_c^2} ・ \frac{s(sT+1)}{T} }$

${ \displaystyle = \frac{\omega_c^2 \tau s^2 + \omega_c^2 s}{s^2+\sqrt{2}\omega_c s + \omega_c^2} }$

このように分母次数と分子の次数が等しくなり, $F(s)G(s)^{-1}$ は (なんとか) 動作します。

MATLAB/Simulinkでシミュレーション・実行する際に, 逆システム（を離散化したもの）をそのままの伝達関数として扱うことはできません。(プロパーでない（非因果モデルに使用できません）とでます)

よってこのような方法を取っています。

動作検証

あらかじめシステム同定を行い, パラメータを取得しておきます。

$K = 570.860$

$T = 0.5311$

ゲインの計算とオブザーバ（とフィルタ）の設定と実行します。

% pos_pd_mbd.m
s = tf('s');

% Initialize & load data
close all
clear all
load sim_param
load model_data

% PD design by pole placement
omega_n = 10;
zeta    = 0.65;
p1 = (-zeta + j*sqrt(1-zeta^2))*omega_n;
p2 = (-zeta - j*sqrt(1-zeta^2))*omega_n;

% Set PD parameters
Kp = p1*p2*T/K;
Kd = -((p1+p2)*T + 1)/K;
Ki = 0;

% observer setting
omega_c = 7;        % cutoff frequency
Fs = omega_c^2 / (s^2 + sqrt(2)*omega_c*s+omega_c^2);
Fsd = c2d(Fs,ts,'zoh');
[numfsd,denfsd] = tfdata(Fsd,'v');  % discrete filter
invP = s*(T*s + 1)/K;   % inverse model
Est = Fs * invP         % estimation
Estd = c2d(Est,ts,'zoh');
[numestd,denestd]  =tfdata(Estd,'v'); % discrete model

% Experiment
r      = 60;
r_cyc  = 4;
dist   = 2; % 0 or 2 for dist test
% dist = 0;
Ncyc   = 4;
tfinal = r_cyc*Ncyc;
open_system('pos_pid_mbd_sl_observer')
open_system('pos_pid_mbd_sl_observer/Scope')
sim('pos_pid_mbd_sl_observer')