微分器とは
初めまして。よろしくお願いいたします。
近似微分器とは、微分器にLPF:ローパスフィルタを追加したものを言います。
微分器は入力信号を微分します。
例
- 直流 -> 0
- $sin(wt)$ -> $wcos(wt)$
微分器をラプラス変換するした時の伝達関数は$s$です。 $s=jw$（$w$各周波数，$i$虚数）を代入することで，周波数特性を計算すると
左：ゲイン 右：位相
微分器の特徴
- 高周波ほどゲインが比例的に増加
- 位相はPI/2だけ追加
これが微分器の特性です。
ローパスフィルタ
有名なので，軽くですが，高周波成分をカットするフィルタです．
離散微分器
最も単純な微分方法は，
{(今回の計測値) - (前回の計測値)}/(計測時間)
だろう．
しかし，微分器は，先ほどの微分器$s$とは異なった伝達関数となる．
- 高周波ほど位相が$-\pi/2$からずれる
- ゲインも高周波ほど，周波数に対する比例性が弱まっていることがわかります．
数式的には
ノルムは $2sin(w/2)$ となります。
位相は $PI/2 - w/2$
であり，最大の周波数（サンプリング時間を$T$とすると，$2/T[Hz]$）では，増幅率2倍のフィルタになっている．
この方式の問題点は
- ノイズ（高周波成分）が支配的になり，微分したい信号の微分値が埋もれる
事である．
離散システムについて
先ほどの微分器の周波数解析の方法について説明する．
この離散微分器は次のようにz変換できる．
\frac{1-z^{-1}}{T}
zは時間進み演算子である．
信号値列$s[0],s[1], \dots,s[n]$に対し，$z$を作用させると
z *s[0] = s[1]\\
z*s[1] = s[2]\\
z^{-1}*s[1] = s[0]\\
のように，信号値の過去・未来の数値を参照することを意味する．
\frac{1-z^{-1}}{T} * s[t] = \frac{s[t]-s[t-1]}{T}
（今 ー 過去）/サンプルタイム と微分の概念そのままである．
周波数特性の出し方
$z=exp(jw)$を代入すれば計算できる．
やる夫で学ぶデジタル信号処理がおすすめ．
$1-z^-1$ に $z=exp(jw)$ を代入すると数式上、計算結果は
ノルムは 2sin(w/2)
位相は PI/2 - w/2
離散微分器の実際
微分器は分解能の影響が大きく感じます。
例：
$0v$と$1v$しか取らず$Tsamp ＝ 0.01$ で$\frac{1-z^{-1}}{T}$ という離散化微分器ならば
$100[v/s]$か$0[v/s]$しか取りません
値の上下がかなり大きく，実用可能とは見れない結果が出てしまいます．
Tsampの値が小さいほど、微分器の出力も敏感でかなりばたついた結果になります。
また、元データに高周波のノイズが乗っていると、微分器は高周波ほどゲインが大きくなる構造上ノイズが真値の微分を覆い隠すほどのものとなってしまう場合があります。こういった場合ほど、LPFの効果が大きく現れます。
実際にはノイズと真値の周波数の区切りというのはわからないものですが・・・
あと、位置の分解能が1mm、サンプリング時間が0.01sなら
速度は0,100,200,・・・[mm/s]という取り方をするので、速度分解能は100mm/sといえます。対象物の速度が10mm/s以上を取らない場合、オイラー法による微分器は分解能から使い物になりません。これはＬＰＦを通せば使い物になるのかというとわかりません・・・
ただ、サンプリング時間を延ばすのはデータを捨てる事と同じなのであまりしたくないです。LPF（ローパスフィルタ）を通す際に１サンプルより前の値を使用するのでなんとかなるかなぁ
設計法
設計方法は、
- s領域で設計したLPFに$s$を掛け双一次変換（他にも方法はあるが）
- z領域で作ったLPFに$\frac{1-z^{-1}}{T}$ を掛けて作る
の2つを紹介します．
近似微分器 : s領域：連続型
五次バタワースフィルタをLPFとします。Scilabを使います。
F=\frac{1}{(s^5)+3.246*(s^4)+5.236*(s^3)+5.236*(s^2)+3.246*s+1}
がs域でのLPFです。（1Hzでカット）
周波数応答は
- ゲインは，$1Hz$以降は急激に落ちる
- 位相については$-\pi$~$\pi$に直線的に変動 しているのがわかります。
sを掛け微分器とした場合
F=\frac{s}{(s^5)+3.246*(s^4)+5.236*(s^3)+5.236*(s^2)+3.246*s+1}
伝達関数を計算すると
左：ゲイン 右：位相
バタワースのカットオフ$1Hz$までは比例が現れ、それ以上の高周波ゲインが0になっていくのが現れています。位相特性は$\pi/2$->$-\pi/2$まで$0<w<wcut$の間で変化している.
双一次変換
双一次変換を使い離散システムへ変換．双一次変換は
s=\frac{2}{T}*\frac{z-1}{z+1}
を代入すればできる．$s=jΩ$ ,$w = exp(jw)$を代入すると
Ω = 2/T * Tan(w/2)
となります。これはwが0に近いときは $Ω = w/T$ であり。
0付近では離散システムと連続システムの周波数応答は一致し、
Ωが大きくなって行くのは、wがPIに近づくとTan(w/2) が無限大に向かうことが対応します。
つまり、双一次変換は周波数応答が近いシステムに変換する方法の一つです。
近似微分器 : s領域：離散型
双一次変換をし，周波数特性を計算すると次のようになる．T=0.01s、cutof=1radで行ってます。
予想どおりでできています。DTFT,TastinのScilab1のプログラムは別に投稿してます。
実際に使用
振り子の先端に加速度ジャイロセンサ、根元に角度測定用エンコーダー（分解能1°）の二つを付けています。
最近、mbedを始めたのですが、SPIよりSerialで通信させるのがベストですね！SPI受信割り込みを持たないので当然かも知れませんが、Serialを使うとエラーもなく安定して通信できます。ただ、複数Byteの通信だとプロトコルが複雑になってしまって面倒です。なにか方法はないかな・・・あと、Arduinoのシリアルプロッタサイコ――！！
エンコーダーの出力を微分し、ジャイロセンサの出力と比較し微分器の性能を比較します。
実験結果は
青：ジャイロ 赤:オイラー法による、エンコーダー微分
形状はかなり正しく、計算できているといえるでしょう。だた、やはり分解能の問題でばたついています。エンコーダーによる角速度のオイラー法による分解能は1deg/0.01s = 1.745rad/sとなります。
実際、グラフの赤線は1.745の倍数の値しかとりません。今回の実験では10rad/s以上を取らないので、このままではエンコーダ由来の角速度は13段階の値しかとらない制度に掛けるものとなってしまいます。
近似微分器設計
T = 0.01; //サンプリング時間Tastinにつかう
wa = 50; //実カットオフ周波数：バタワースの設計時
function[F] =Hz(z);
F=z*wa/((z^5)+3.246*(z^4)+5.236*(z^3)+5.236*(z^2)+3.246*z+1); //5次バタワースフィルタ
endfunction
function [F]=Hz1(z); //カットオフ周波数変更
F=Hz(z/wa);
endfunction
function [s]=Tas(z);
s=2/T*(z-1)/(z+1);
endfunction
w1 = -1*%pi:2*%pi/1000:%pi; //周波数軸
for i = 1:1001
k = Hz1(Tas(exp( %i*w1(1,i)))); //双一次変換後
wd = 2*atan(wa*T/2);
y(i)=abs( k ); //振幅
y2(i)=atan(imag(k),real(k)); //位相
end
disp(wd);
disp(%pi/wd);
disp('プリワープ後 カットオフ周波数');
figure(1);
plot(w1,y');
figure(2);
plot(w1,y2');
z = poly(0,"z");
//k= Hz1(Tas(z));
k=Hz1(Tas(z));
r =denom(k);
m=roots(r);
disp(r);
disp(m);
disp(k);
これで、差分方程式を設計できる（連続領域で近似微分器をつくってから双一次変換をする方式）。
実際に使用：近似微分器
T=0.01sで全て実験する。
Wa = 50 ( 正規化 0.5)
形状一致、ノイズもない。が位相遅れがある。
wa = 100( 正規化 1)
形状、位相ともにかなり良いが、ノイズが結構乗ってしまっている。
wa = 150 ( 正規化 1.5)
あまり変わらなかった
wa = 300( 正規化 3)
だいぶノイズが乗るようになった。
最適なLPFの区切りは確実にあるので位相と話あいながら調整する必要があるという事がわかる。
二階微分
wa = 50 で設計
青：ジャイロ 赤:エンコーダー2階微分
なんとなく結構それっぽいデータを算出できている気がする。
ただ、結構ノイズっぽいのが乗っている。
青：ジャイロ出力に近似微分器
赤：システムを同定し角加速度を角速度（ジャイロより）と角度(エンコーダーより)であらわし予測する
緑：エンコーダーの二階近似微分器 wa = 0.5
この３つを比較をすると、
形状はエンコーダーの二階微分したものは結構ノイズが載っていますが、形と大きさはだいたい同じでかなりいい計算ができていると思います。
位相は赤：緑：青の順に早いです。
エンコーダー分解能1°を微分器を通すと1階でも2階でも値が小さいとき速度、加速度分解能が大きすぎるため100と0を行き来するような激しい反応がやっぱり消えません。
角加速度 = -0.76952*角速度-60.2556*sin(角度)
から予測しました。オイラー法とソルバーを用いてパラメータを同定し予測しました。結構よい予想となっていると思います。
ルンゲルクッタ法を使えばまた違った結果になるかもしれません。
ジャイロを用いた現在角度予測
ジャイロの角速度を積分すれば予測できます。やってみましょう！
悪くはないですが、時間がたつと定常偏差が出てしまうのでやはり難しいですね。ジャイロの平均値を引いて補正すると、多少良くなります。
加速度センサを加える。
振り子の場合
張力方向の加速度axは (R=0.162m)
R*w^2+g*cos(θ)
張力垂直方向の加速度ayは
R*(dw/dt)-g*sin(θ)
で計算できます。
青：ジャイロとエンコーダーからの推定加速度ax 赤:計測加速度
結構、ジャイロとエンコーダーから精度よく推定できています。誤差の要因は
- 加速度センサの角度偏差が存在
- 加速度センサの加法的・乗法的誤差
があると思っています。何とかこの誤差パラメータを推定し、補正できれば便利なのですが・・・
ayの場合は
青：-g*sin(θ) 赤:-1*R*(dw/dt) 緑:計測加速度
まず、加速度センサの取り付け角度偏差によって緑は初期0をとっていないです。
振り子の場合、計測加速度は赤の重力由来と青の角加速度由来が打ち消しあって振り子として大きく揺れていても加速度が計測できないです。緑が偏差＋0付近で推移していてあまり使えない。
と思ったら、ちょうど加速度センサーの設置しているRがちょうどいい場所にあったため
ちょうど打ち消す位置にあって、Rを小さくしたところ問題なくなりました。
実際に加速度センサの位置をR=0.1m
青：-g*sin(θ)-1*R*(dw/dt) 赤:計測加速度
悪くない推定である気がしますが、 sin(θ)の項があるとθが0付近で揺れるとき、取り付け角度誤差が顕著に表れてしまうのではないかと思われます。
シミュレーションと実測値の誤差から、取り付け角度、安定位置と取り付け位置の偏角をエクセルのソルバーを用いてとくと
と導出できました。この数値を用いて計測加速度ax,ayを推定すると
いろいろ頑張ってみましたがsinθのあるayの項を推定するのはかなり難易度が高いと感じました。axのほうのみからθを推定するほうが簡単かもしれません。
青：計測Ay 赤:推定Ay 緑:計測Ax 黄:推定Ax
張力垂直方向の加速度ayは
R*(dw/dt)-g*sin(θ) ですが、(dw/dt) と sin(θ) は位相差がPIで打ち消しあう位置に常時あるので、計測加速度はとても小さくなる傾向がありますが、実験推定計算加速度では計算(dw/dt) の位相は先にあげたグラフより
青：ジャイロ出力に近似微分器
赤：システムを同定し角加速度を角速度（ジャイロより）と角度(エンコーダーより)であらわし予測する
緑：エンコーダーの二階近似微分器 wa = 0.5
まぁまぁ位相がずれているためうまくいかないと予想されます。
加速度推定に使う角加速度をシステム同定(自由振動)によって得た角速度を使用すると
青：計測Ay 赤:推定Ay 緑:計測Ax 黄:推定Ax
結構制度の良い結果が得られています。しかし、自由振動の場合にしか利用できないのと、システム同定が必要になるので現実的ではないです。
ジャイロの近似微分器のカットオフwa を 0.5から 3.0にして位相ずれを減らすと
誤差の山の高さが減りましたが、まだこれでは使えないレベルの誤差が載ってしまっています。
手で激しく揺らすほうでは精度よく推定できていました。
手で激しく揺らす場合、加速度は大きく,角度は小さくなるので、片方が卓越して表れて、これまでの重力項と角加速度項のぶつかり合いが減って精度が上がったと考えます。
張力方向の加速度axは (R=0.162m)
R*w^2+g*cos(θ)
こっちを使う場合は θがcosでかかってくる項であるため、θ=0,180では推定θの精度があまりよくない可能性があります。
gx（青）の成分を遠心力由来（赤）と重力由来（緑）に分離しました
手で回転しないようにしながら動かしたところ、重力項と計測加速度が一致しているので分離はうまくできていると思われます。しかし、加速度が大きくなるとあまり合わないです。
角加速度と角度を眺めていたところ角加速度が遅れていいると思われたので、推定に使う角度を4サンプル遅らせることで対応させてみると
完璧な推定ができるようになりました。4サンプルの前後の3,6で試すと
3では
5では
微妙うにうまくいっていないのがわかります。
以上より、とりあえず計測数値の関係性と満足する微分器や方程式を生成できたので満足しました。
よく考えたら、加速度センサに使っているMPU6050の設定で、内蔵のLPFを5hz,delay19msと設定して使っていました。この設定のせいかもしれないので、一応チェックしてみました。
mbedプログラム
解説はないですが、使ったプログラムを張っておきます。
#include "mbed.h"
#include "Serial.h"
Serial pc(USBTX, USBRX); // USBシリアルポートのインスタンス
Serial Ro(p9,p10);
Serial Ac(p13,p14);
DigitalOut myled(LED1);
DigitalOut check(p18);
InterruptIn css(p17);
char low=0;
char high = 0;
int Rotate = 0;
bool Polarity = true; //true:負
int DataAbit = 0;
volatile int DataA[16];
int gx;
int gy;
int gz;
int ax;
int ay;
int az;
bool TF = false;
float R[7] = {0,0,0,0,0,0,0}; //単位補正後 角度
float W[6] = {0,0,0,0,0,0}; //角度微分1階と2階
float W2[6] = {0,0,0,0,0,0}; //ジャイロ微分
float GZ[7] = {0,0,0,0,0,0,0}; //単位補正後 ジャイロ
float AngleGyro = 0; //ジャイロよりの角度
float Radi = 0.1; //半径[m]
float AX;
float AY;
void cssf() //加速度センサ 1 p9
{
DataAbit = 0;
}
void getR()
{
//pc.printf("%c ",Ro.getc());
char Rse = Ro.getc();
if((Rse&0b10000000)==0b10000000) { //highByte
high = Rse ;
if((high&0b01000000)==0b01000000) { //値は負なのでそのまま
Polarity = true;
} else { //値は正なので最上位を0にする
high= high & 0b01111111;
Polarity = false;
}
//pc.printf("%3x high",Rse);
} else { //lowByte
low = Rse;
//pc.printf("%3x low",Rse);
Rotate = (high <<7) | low ;
if(Polarity ==true ) { //負の時 int 2byte でないので
Rotate = Rotate|0x8000;
Rotate =( 0xFFFF - Rotate) * (-1) -1 ;
//Rotate = -1* (~Rotate)-1;
}
//pc.printf("%6d \r\n",Rotate);
for(int i = 6; i>=1; i--) {
R[i]= R[i-1];
}
R[0]= Rotate*-1*0.01745329;
}
}
void getA()
{
DataA[DataAbit]= Ac.getc();
DataAbit ++ ;
if(DataAbit == 12) { //全データ
DataAbit = 0;
for(int i = 0; i<6; i++) {
DataA[i]= DataA[i*2]<<8 | DataA[i*2+1];
if( (DataA[i]& 0x8000) == 0x8000) { //負の時
DataA[i] = (0xFFFF - DataA[i] ) *(-1) -1;
}
}
gx = DataA[0];
gy = DataA[1];
gz = DataA[2];
ay = DataA[3] * -1;
ax = DataA[4];
az = DataA[5];
TF = true;
for(int i = 6; i>=1; i--) {
GZ[i]= GZ[i-1];
}
GZ[0]=gz * 2.66316109 * 0.0001 - 0.002066694;
AX = ax * 5.987548 * 0.0001;
AY = ay * 5.987548 * 0.0001;
}
}
int main()
{
pc.baud(115200);
Ro.baud(115200);
Ro.attach(getR,Serial::RxIrq);
Ac.baud(115200);
Ac.attach(getA,Serial::RxIrq);
css.rise(&cssf); //p9
while(1) {
if(TF == true) {
TF = false;
check = 1;
/*
pc.printf("%8f ",GZ[0]);
pc.printf("%8f ",AX);
pc.printf("%8f ",AY);
pc.printf("%8f ",R[0]);
*/
/*
float w = gz * 2.66316109 * 0.0001;
float ty = -1*ay*5.9875488*0.0001;
float tx = (ax*5.9875488*0.0001-0.162*w*w);
float s = atan2(ty,tx);
pc.printf("%5f ",s);
*/
//pc.printf("%5d \r\n",Rotate*-1);
//W[0]=(R[0]+4*R[1]+5*R[2]-5*R[4]-4*R[5]-R[6])-(-78.2*W[1]+111.16*W[2]-80.91*W[3]+30.12*W[4]-4.563*W[5]);
//W[0]= W[0]/22.878;
//近似微分器：離散した、バタワースに離散か微分器を掛けた者
/* wa = 0.5
W[0] = (0.0874187*R[0] +0.2622561*R[1] + 0.1748374*R[2] + -0.1748374* R[3] -0.2622561*R[4] -0.0874187 *R[5]);
W[0] = W[0]-(-3.4182041*W[1]+4.8518078*W[2]-3.5366809*W[3]+1.3165256*W[4]-0.1994615*W[5]);
*/
/*wa = 1.0
W[0] = (1.2965124*R[0] +3.8895371*R[1] + 2.5930248*R[2] -2.5930248* R[3] -3.8895371*R[4] -1.2965124*R[5]);
W[0] = W[0]-(-2.0243485*W[1]+2.082147*W[2]-1.1488656*W[3]+0.3406457*W[4]-0.0421367*W[5]);
*/
//wa = 1.5
//W[0] = (4.8168793*R[0] +14.40638*R[1] + 9.6337586*R[2] -9.6337586* R[3] -14.450638*R[4] -4.8168793*R[5]);
//W[0] = W[0]-(-0.8902957*W[1]+0.9121829*W[2]-0.3389243*W[3]+0.0970306*W[4]-0.0092928*W[5]);
//wa = 3.0
//W[0] = (25.59053*R[0] +76.771591*R[1] + 51.181061*R[2] -51.181061* R[3] -76.771591*R[4] -25.59053*R[5]);
//W[0] = W[0]-(1.2393125*W[1]+1.1733521*W[2]+0.5228261*W[3]+0.143486*W[4]+0.0155081*W[5]);
//二回微分 wa = 0.5
W[0] = (17.48374*R[0] +17.48374*R[1] -34.96748*R[2] -34.96748* R[3] +17.48374*R[4] +17.48374*R[5]);
W[0] = W[0]-(-3.4182041*W[1]+4.8518078*W[2]-3.5366809*W[3]+1.3165256*W[4]-0.1994615*W[5]);
//gz の近似微分器wa =0.5
//W2[0] = (0.0874187*GZ[0] +0.2622561*GZ[1] + 0.1748374*GZ[2] + -0.1748374* GZ[3] -0.2622561*GZ[4] -0.0874187 *GZ[5]);
//W2[0] = W2[0]-(-3.4182041*W2[1]+4.8518078*W2[2]-3.5366809*W2[3]+1.3165256*W2[4]-0.1994615*W2[5]);
//gz の近似微分器wa =3.0
W2[0] = W2[0] = (25.59053*GZ[0] +76.771591*GZ[1] + 51.181061*GZ[2] -51.181061* GZ[3] -76.771591*GZ[4] -25.59053*GZ[5]);
W2[0] = W2[0]-(1.2393125*W2[1]+1.1733521*W2[2]+0.5228261*W2[3]+0.143486*W2[4]+0.0155081*W2[5]);
//近似微分器：離散化前に微分器を掛けた
//W[0]= (R[0]-R[1])/0.01; //オイラー法
//同定システムより角加速度予想 rad/s^2単位
float Pw2 = -1.06978*GZ[0]-73.7848 *sin(R[0]);
//rad
//pc.printf("%5f ",gz * 2.66316109 * 0.0001);
//pc.printf("%5f \r\n",W[0]);
//deg
//pc.printf("%5f ",W2[0]);
//pc.printf("%5f ",Pw2);
//pc.printf("%5f ",W[0] );
//角速度積分による角度予想
//AngleGyro += GZ[0]*0.01;
//pc.printf("%5f ",AngleGyro * 57.29577951);
//pc.printf("%5d \r\n",Rotate*-1 );
//加速度
//float AXw = Radi * GZ[0] * GZ[0] + 9.81 * cos(R[0]);
float AYw =-1*Radi*W2[0] - 9.81 * sin ( R[0]); //-1*Radi*W2[0]
//pc.printf("%5f ",AYw);
//pc.printf("%5f ",-1*Radi*W2[0] );
//pc.printf("%5f \r\n",AY );
//加速度推定 取り付け偏差、安定偏差補正 Radi
int k = 4; //位相遅れ補正用
float bb = 0.01740; //rad:安定偏差
float rr = -0.185744; //rad:取り付け偏差
float cbb = cos(R[k]-bb);
float sbb = sin(R[k]-bb);
float cbr = cos(R[k]-bb-rr);
float sbr = sin(R[k]-bb-rr);
float axx = Radi*(cbb*GZ[0]*GZ[0]+sbb*W2[0])+9.5839;
float ayy = -1*Radi*(sbb*GZ[0]*GZ[0]+cbb*W2[0]);
float axxx = cbr * axx + sbr * ayy;
float ayyy = -1*sbr * axx + cbr * ayy;
//axxxの加速度由来と重力由来を比較する
float axxgn = Radi*(cbb*GZ[0]*GZ[0]+sbb*W2[0]);
float axxgy = +9.5839;
float axxxgn = cbr * axxgn + sbr * ayy;
float axxxgy = cbr * axxgy + sbr * ayy;
//pc.printf("%8f ",R[5]*30);
//pc.printf("%5f ",GZ[0]);
//pc.printf("%8f ",W2[0]/4);
pc.printf("%5f ",AY);
pc.printf("%5f ",ayyy );
pc.printf("%5f ",AX);
pc.printf("%5f \r\n",axxx );
/*
pc.printf("%5f ",AX);
pc.printf("%5f ",axxxgn );
pc.printf("%5f \r\n",axxxgy );
*/
for(int i = 5; i>=1; i--) {
W[i]= W[i-1];
}
for(int i = 5; i>=1; i--) {
W2[i]= W2[i-1];
}
check = 0;
}
}
}
LPFのステップ応答・位相遅れ
LPF＋微分器を使う際に、ステップ入力のような急峻な変化をする点がある場合振動するような応答をする場合があります。簡単のためにLPFのみでステップ入力を与えます。
5次バタワースフィルタ：カットオフ1.0
青：入力 オレンジ：出力
遅れ＋振動が付与されてしまっています。
入力はw=2*PI()/10=0.628 であるので位相遅れは-PI/2 つまりsin とcosのような関係になります。
LPFとして2次バタワースを使うと wcut = 1.0
先ほどより立ち上がりが速く、振動も割と少ない？気がします。
FIRフィルタを使用した場合このような振動は起こらない気がします。
実際、3点平均化フィルタでは
振動しません。
平均化フィルタ＋微分器
これの計算結果は、
(1+z^-1+z^2)/3 * (1-z^-2)/T
=(1-z^-3)/(3*T)
つまり、サンプリング間隔を伸ばして計測した場合の離散微分器と同じ事になります。
T=0.01で入力にはステップを与えています。1/(3*0.01)=33.33より、微分器は最大値33を取っています。
sin波を与えると w=2PI()/50=12.5 T=0.01
それっぽい結果になっています。