スポンサーサイト

上記の広告は1ヶ月以上更新のないブログに表示されています。
新しい記事を書く事で広告が消せます。

慣性のリアルタイム更新と加速度フィードフォワード

今日はイナーシャ推定の続き.
マイコンに先の同定アルゴリズムを実装し,パラメータ等を変えながらモータ駆動実験を繰り返す.

推定したイナーシャを元に,加速度フィードフォワード値をリアルタイムに変化させてみる.
フィードバックゲインは変えない.
イナーシャ推定は逐次最小二乗法を用いた.



この例では,イナーシャをおよそ9倍に変化させたときの速度応答を見ている.

ここで,イナーシャが更新される図の赤線(100msec経過)あたりでサーボ動作が乱れ,一時的に速度誤差が大きくなっている.
その後は誤差はゼロに収束し,同定が完了した2回目以降はほぼ速度誤差はゼロで安定している.

このことから,同定しながらFF値更新すると,一時的にイナーシャ値を固定したときよりも追従特性が悪化する場合があることが分かった.
フィードバックをいじらなければもっと素直な応答(誤差が同定の進行と共に漸減)になると予想したが,意外な結果だ.
今回の例でいうと,もともとイナーシャが真値よりも小さい状態から駆動を開始するため,起動直後はFF制御量が足りずFBの負担が大きいためサーボは振動的になる.
その状態の時にイナーシャ推定値更新すると,フィードフォワードトルクが急激に増加しますます不安定になる,ということだろうか.
もしそうだとしたら,フィードバックの振動が十分収まってからイナーシャ値を更新すれば,少しはマシになるかもしれない.

リアルタイム慣性同定のテスト

今週末はリアルタイム同定アルゴリズムの実装準備を行う.

やりたいのは加速度と電流値の測定値からシステム全体のJ/Kt=[慣性モーメント/トルク定数]を逐次推定することである.
この定数は電流とトルクを結びつけるサーボの最も基本的なパラメータであり,
ここからフィードバックゲインやフィードフォワード量を決定することができる.
これをリアルタイム推定できれば,例えばロボットが急に重いものを持ったりしても,持つ前と全く同じ動きをするシステムが実現できる.
マイコンに実装する前に確認のため,エクセルで計算し感じをつかむ.
入力値はモータをSinカーブの速度目標値で駆動させた時の加速度と電流値である.


上記の加速度と電流値はともに50Hz程度のローパスをかけてノイズを下げてある.
このデータに対し,最小二乗法(RLS),固定トレース法(FTA)でJt/Ktを逐次推定した結果が以下である.

逐次最小二乗法(Recursive Least Square=RLS)は忘却係数により新しい値にどのぐらい重みを付けて推定するかを選ぶことができる.
忘却係数を1.0にすると,その時までの過去データをすべて均等に使った区間平均と等価になる.
通常は1.0よりわずかに小さい値に設定し,イナーシャ変動に追従できるようにする.

固定トレース法(Fixed Trace Algolism=FTA)では加速度が大きいときは重みが増加し,加速度が小さい=S/N比が悪いときは新しいデータの重みが下がって前回値を保持する特徴がある.
このため有効なデータがノイズに埋もれ,推定値がノイズの平均値に引っ張られるのを回避することができる.

上記のグラフで,速度が小さい時はエンコーダ分解能の不足による推定精度の劣化が見られる.
ここは瞬時速度オブザーバで精度UPするか,単純に低速域の重みを下げるかで対応すればよい.
しかし,いろいろいじる中で気になったのは外乱の影響である.
上記は外乱ゼロで,摩擦負荷は事前に同定した値を使っているので安定しているが,実際には負荷変動や摩擦の変動影響があると推定値が大きく振られてしまう.
この文献によると,外乱に強くするには加速度の微分を使いDCカットする方法,外乱オブザーバで推定した推定外乱トルクを組み込む方法が有効らしい.
なるほど!ということでさっそくやってみたところ,加速度の微分=躍度のデータを入力にした場合はノイズが大きすぎて推定値はほぼゼロに収束してしまった.
外乱代入による方法も試したが,瞬時速度オブザーバのイナーシャ値が発散してしまい,こちらもうまくいってない.
何かやり方がまずいのかもしれない.

そもそも,外乱負荷と全イナーシャ変動の違いって区別できるものなんだろうか?
例えばモータに力を加えた時にモータ軸が前よりも「動きにくい」という状態だった場合に,それが一時的に加わった負荷によるものなのか,それともシステムの慣性が増加したものなのか,どうやって判別すれば良いものか.
例えばロボットアームの先端におもりをぶつけた場合と,おもりを取り付けた場合の差は何か.
差があるとしたら,負荷変動は作用時間が短く,慣性変動は長い,ということだろうか.
だとしたら,イナーシャの推定速度は外乱負荷の推定速度よりも十分遅くしないといけないことになる.
追従が遅いのではイナーシャ値を逐次更新するメリットがあまりないのではないか.
それだったらいっそのこと,イナーシャの逐次推定はやめて初期値固定にしておいて,外乱オブザーバで慣性誤差も外乱負荷もまとめて全部出力しちゃったほうが手っ取り早い気もしてくる.
でもそうなると今度は,イナーシャが長い間オフセットしたままになった場合にフィードフォワード量が実システムに合ってないので恒常的に精度が悪くなるなぁ・・・
ニワトリが先か,タマゴが先か・・
うーん・・・実はこのあたり,まだ全然イメージがつかめてない.
いずれにしても,実験をやりながら少しずつ解決していこうと思う.

瞬時速度オブザーバ,イナーシャ同定など

だいぶ更新が止まってしまったが,ここ最近はモータ制御アルゴリズムの改良に取り組んでいる.
追値制御の速度精度はリアルタイムのエンコーダ誤差補正でかなり滑らかになったが,低速域の残留振動が気になっていた.
 これまでにエンコーダパルスの極性反転を無視する不感帯を入れたり,超低速までカバーできる大容量のInput-Captureタイマを使用するなどして改善を試みてきたが,駆動慣性が極端に小さい場合やエンコーダが1制御サイクルに1pulseも来ないような低速ではどうしても振動が生じてしまう.
この原因はエンコーダパルスの分解能が不足していることにある.
先の実験で、エンコーダ分解能を上げれば改善することは分かっているが,そうすると要求される計算リソースがそれに比例して大きくなってしまう.(また,エンコーダの値段も当然上昇する.)
あるいは低速時にカクカクになる速度信号を滑らかにしようと,安易にLPFを挿入したりすると追従速度も遅くなってしまい,せっかくのコアレスモータの機敏な応答が失われてしまう.
なんとか応答速度は高速をキープしたままもっと滑らかな速度値を得られないものか・・・

そこで,制御周期内にパルスが来ない区間の速度をモデル推定する 瞬時速度オブザーバ を実装してみた.
以下がその実験結果の一部.


グラフはSin波形で目標速度を変化させた時のモータ速度の追従精度を表している.
瞬時速度オブザーバ(Instantaneous Speed Observer =ISO)をONにすると,低速域の速度リプルが小さくなっていることがわかる.
とくに制御周期の2~4倍の間エンコーダのパルスが来ないような超低速域で,速度がカクカクになって振動してしまうのを防ぐ効果がある.
このことは,例えば同じイナーシャに対して速度F/Bのゲインを高くした場合の振動抑制にも効果があることを示している.
また,制御周期をDSP等でさらに高速化すれば低速だけでなく中速,高速域でも速度制御を滑らかにする効果がある.

瞬時速度オブザーバでは,パルスが来ない区間の速度を電流の積分値から計算した速度推定値で補うことができる.
つまりいつでも好きなタイミングで「当たらずしも遠からずの速度の瞬時値」が得られる.
ただし,この電流積分から得られる推定速度は外乱等の影響でどんどんドリフトするので,エンコーダパルスが来た瞬間に真値に近づくように補正してやる必要がある.
推定値の修正はパルス発生ごとに行う方法と,制御周期ごとに行う方法があるが,パルス発生ごとに割り込み等で行ったほうがより正確な値に近くなる=そのぶんより低速まで安定して推定できる.

ISOを使うには①電流検出精度と,②イナーシャや摩擦抵抗の正確な同定が重要になる.
今回①は自作のモータドライブ基板に差動アンプや高精度ADCをフル活用して精度を確保しているため問題ない.
②は今のところ台形速度駆動や一定速度駆動時の電流値などからオフラインで算出しているが,少々面倒なので今後はオンラインで同定できるように,逐次最少2乗法などを実装していきたい.

TeraTermの簡易グラフィック出力

今日はTeraTermのグラフィック出力をテストする.
シリアル通信経由でのデバッグやデータダンプ用にWindows-XP以降はHyperTerminalが無くなったので,フリーソフトのTeraTermを使っているユーザは多いと思う.
16bit,32bitマイコンではデータ量が大きくなるため,数値データ列をいちいちエクセルにコピペしてグラフ化するのが面倒だった(具体的には長~いデータをマウスでドラッグするのが面倒!).
もっとマイコンからダイレクトにグラフ描画やXYプロットなんかができないものか・・・

そんな矢先,ふとO-Familyさんの Tektronix 4010(4014) グラフィック・ターミナルへ描画の記事を見つけ,さっそく試してみることに.




こんな感じで別ウィンドウが開きグラフィック表示できる.(TeraTermのバージョンは4.84.)

使い方はTeraTermのSetUP→Terminalと進み,設定ダイアログの中ほどに[Auto Switch VT<->TEK]にチェック.
カラー表示したい時はTEKウィンドウメニューのSetup→Windowと進み[color emuration]にチェック.
扱えるのは基本的に点と直線のみ.色は16色と超シンプル.
いわゆるASCIIコードのエスケープシーケンスでベルを鳴らしたりするのと同じ感覚で使えるため,マイコンで簡単に描画できる.

O-FamilyさんのライブラリはAVR用のアセンブラだったので,これをC言語でSH4A用に書き換えた.
上記スクリーンを出力した際のコードを参考に載せておく.
※spi_put()の関数は自分がSPI-USB変換を使っているためSPI出力関数です.
SCI,UARTなどお使いのシリアル出力関数に適宜置き換えてください.
TEK4010コマンドの詳細は こちらのフォーラム を参考にしました.
/*
TeraTermのTek4010描画エミュレータを使いグラフィック表示を行う
TeraTermのVer4.84以上推奨
sci.c もしくは hspi.cを先にインクルードしておくこと

*/
int Get_RGB(int r,int g,int b)
{
int colour;
r=(r<<16)&0xFF0000;
g=(g<< 8)&0x00FF00;
b=(b<< 0)&0x0000FF;
colour=(r+g+b)&0xFFFFFF;
return colour;
}
//*************RGB888→bgr111***********************
int rgb888tobgr111(int colour) {
int r = (colour >> 23) & 1;
int g = (colour >> 15) & 1;
int b = (colour >> 7) & 1;
return (b << 2) | (g << 1) | (r << 0);
}
//**********表示色を変えるエスケープシーケンス*******************
//TEKウィンドウの[SetUP]→[Window]ダイアログの中の[color Emuration]にチェックを付けておく
void Change_ForeGround_Color(int color)
{
int c;
c=30+rgb888tobgr111(color);
spi_printf("\033[%dm", c);
}
//*********文字背景色を変えるエスケープシーケンス***************************
//
void Change_BackGround_Color(int color)
{
int c;
c=40+rgb888tobgr111(color);
spi_printf("\033[%dm", c);
}

//**********TEK4010モードへ入る*********
void Enter_Tek4010(void)
{
spi_putc(0x1B);
spi_putc('[');
spi_putc('?');
spi_putc('3');
spi_putc('8');
spi_putc('h');
}
//**********TEK4010モードから出る*********
void Exit_Tek4010(void)
{
spi_putc(0x1B);
spi_putc('[');
spi_putc('?');
spi_putc('3');
spi_putc('8');
spi_putc('l');
}

//************画面クリア**************************
void Tek_Clr(void)
{
spi_putc(0x1B); //ESC アルファモードへ戻す
spi_putc(0x0C); //FF
spi_putc(0x0D); //CR
spi_putc(0x0A); //LF
}
//*************文字描画モード(アルファモード)に戻す*************

void Tek_Alpha(void)
{
spi_putc(0x1F); //US アルファモードへ戻す
}
//**********グラフィックモードへ切り替え**********
void Tek_Graph(int vector_no)
{
spi_putc(0x1D); //GS
spi_putc(0x1B); //ESC
spi_putc(vector_no|0x60); //線種を選ぶ
}

/*
直線を引く[xs,ys]→[xg,yg]
座標は左下が[0,0] 幅1023 高さ778まで.
データは10bit+TagBitの構成で全部で8byte送信.

線種の番号
0:標準実線
1:点線
2:一点鎖線
3:短い鎖線
4:長い鎖線
8:Bold実線

*/
void Tek_Line(int xs,int ys,int xg,int yg,int line_no)
{
unsigned char x0,y0,x1,y1;
Tek_Graph(line_no); //グラフィックモードに移行(線種を指定)

x0=(unsigned char)(xs&0x1F); //下位5byteを取り出す
x0=x0|0x40; //Tag-Bitをセット
y0=(unsigned char)(ys&0x1F); //下位5byteを取り出す
y0=y0|0x60; //Tag-Bitをセット

x1=(unsigned char)((xs>>5)&0x1F); //上位byteを5個右シフトしたあと下位5byteを取り出す
x1=x1|0x20; //Tag-Bitをセット
y1=(unsigned char)((ys>>5)&0x1F); //上位byteを5個右シフトしたあと下位5byteを取り出す
y1=y1|0x20; //Tag-Bitをセット

spi_putc(y1); //座標送信
spi_putc(y0); //座標送信
spi_putc(x1); //座標送信
spi_putc(x0); //座標送信


x0=(unsigned char)(xg&0x1F); //下位5byteを取り出す
x0=x0|0x40; //Tag-Bitをセット
y0=(unsigned char)(yg&0x1F); //下位5byteを取り出す
y0=y0|0x60; //Tag-Bitをセット

x1=(unsigned char)((xg>>5)&0x1F); //上位byteを5個右シフトしたあと下位5byteを取り出す
x1=x1|0x20; //Tag-Bitをセット
y1=(unsigned char)((yg>>5)&0x1F); //上位byteを5個右シフトしたあと下位5byteを取り出す
y1=y1|0x20; //Tag-Bitをセット

spi_putc(y1); //座標送信
spi_putc(y0); //座標送信
spi_putc(x1); //座標送信
spi_putc(x0); //座標送信

Tek_Alpha(); //キャラクタモードに戻す
}

/*
Dotを打つ
始点・終点を同じ座標にしたLine引き
*/
void Tek_Dot(int xs,int ys)
{
unsigned char x0,y0,x1,y1;

spi_putc(0x1D); //GS

x0=(unsigned char)(xs&0x1F); //下位5byteを取り出す
x0=x0|0x40; //Tag-Bitをセット
y0=(unsigned char)(ys&0x1F); //下位5byteを取り出す
y0=y0|0x60; //Tag-Bitをセット

x1=(unsigned char)((xs>>5)&0x1F); //上位byteを5個右シフトしたあと下位5byteを取り出す
x1=x1|0x20; //Tag-Bitをセット
y1=(unsigned char)((ys>>5)&0x1F); //上位byteを5個右シフトしたあと下位5byteを取り出す
y1=y1|0x20; //Tag-Bitをセット

spi_putc(y1); //座標送信
spi_putc(y0); //座標送信
spi_putc(x1); //座標送信
spi_putc(x0); //座標送信

spi_putc(y1); //座標送信
spi_putc(y0); //座標送信
spi_putc(x1); //座標送信
spi_putc(x0); //座標送信
Tek_Alpha(); //キャラクタモードに戻す
}

/*
文字の位置を指定する
この関数のの後,文字を出すと文字の原点(左下)がこの位置に合わせられる
*/
void Tek_Char_PosSet(int xs,int ys)
{
unsigned char x0,y0,x1,y1;

spi_putc(0x1D); //GS
x0=(unsigned char)(xs&0x1F); //下位5byteを取り出す
x0=x0|0x40; //Tag-Bitをセット
y0=(unsigned char)(ys&0x1F); //下位5byteを取り出す
y0=y0|0x60; //Tag-Bitをセット

x1=(unsigned char)((xs>>5)&0x1F); //上位byteを5個右シフトしたあと下位5byteを取り出す
x1=x1|0x20; //Tag-Bitをセット
y1=(unsigned char)((ys>>5)&0x1F); //上位byteを5個右シフトしたあと下位5byteを取り出す
y1=y1|0x20; //Tag-Bitをセット

spi_putc(y1); //座標送信
spi_putc(y0); //座標送信
spi_putc(x1); //座標送信
spi_putc(x0); //座標送信
Tek_Alpha(); //キャラクタモードに戻す
}

//************十字カーソルマークを任意座標中心に表示************
void Tek_Cross_Cursor(int xs,int ys)
{
int length=10; //カーソルの長さ
Tek_Line(xs-length/2,ys,xs+length/2,ys,0); //ラインを引く
Tek_Line(xs,ys-length/2,xs,ys+length/2,0); //ラインを引く
}

//************四角形の描画(左下座標,右上座標の2点で囲まれる四角)************
void Tek_Box(int xs,int ys,int xg,int yg,int line_no)
{
Tek_Line(xs,ys,xg,ys,line_no); //ラインを引く
Tek_Line(xg,ys,xg,yg,line_no); //ラインを引く
Tek_Line(xg,yg,xs,yg,line_no); //ラインを引く
Tek_Line(xs,yg,xs,ys,line_no); //ラインを引く
}


void Test_Tek_Graphic(void)
{
int size_x,size_y;
int x,y;
float c;
int i,total,no;
int color1,color2,color3;

size_x=1023; //Tek-Windowの横サイズ
size_y=778; //vTek-Windowの縦サイズ

color1=Get_RGB(255,0,0); //赤
color2=Get_RGB(0,255,0); //緑
color3=Get_RGB(0,0,255); //青

Enter_Tek4010(); //Tekウィンドウを起動(無くてもOK?)

Tek_Clr(); //画面クリア
// Change_BackGround_Color(Get_RGB(255,255,255)); //背景色を白にセット

Tek_Line(0,10,400,10,0); //ラインを引く(始点[x,y],終点[x,y],ラインの種類)
Tek_Char_PosSet(420,5); //文字の位置(左下原点)を指定
spi_puts("Line0\n\r"); //文字を出力

Tek_Line(0,30,400,30,1); //ラインを引く(始点[x,y],終点[x,y],ラインの種類)
Tek_Char_PosSet(420,25); //文字の位置(左下原点)を指定
spi_puts("Line1\n\r"); //文字を出力

Tek_Line(0,50,400,50,2); //ラインを引く(始点[x,y],終点[x,y],ラインの種類)
Tek_Char_PosSet(420,45); //文字の位置(左下原点)を指定
spi_puts("Line2\n\r"); //文字を出力

Tek_Line(0,70,400,70,3); //ラインを引く(始点[x,y],終点[x,y],ラインの種類)
Tek_Char_PosSet(420,65); //文字の位置(左下原点)を指定
spi_puts("Line3\n\r"); //文字を出力

Tek_Line(0,90,400,90,4); //ラインを引く(始点[x,y],終点[x,y],ラインの種類)
Tek_Char_PosSet(420,85); //文字の位置(左下原点)を指定
spi_puts("Line4\n\r"); //文字を出力


Tek_Box(0,0,size_x,size_y,2); //四角形(左下[x,y],右上[x,y],線種)


Tek_Line(0,400,size_x,400,1); //ラインを引く

total=size_x;
for(i=0;i<=total;i++){
c=(float)i/(float)total; //
x=i; //DotのX座標
y=400+(int)(100.0*sinf(c*PI*10.0)); //DotのY座標
Change_ForeGround_Color(color1); //Dotの色を変える
Tek_Dot(x,y); //Dotを打つ

y=400+(int)(100.0*sinf(c*PI*20.0)); //DotのY座標
Change_ForeGround_Color(color2); //Dotの色を変える
Tek_Dot(x,y); //Dotを打つ

y=400+(int)(100.0*sinf(c*PI*30.0)); //DotのY座標
Change_ForeGround_Color(color3); //Dotの色を変える
Tek_Dot(x,y); //Dotを打つ

wait_msec(1); //描画が途切れる場合,適宜ウェイトを入れると改善
}

Change_ForeGround_Color(Get_RGB(255,0,255));
// Change_BackGround_Color(Get_RGB(255,0,255));
x=30;
y=600;
Tek_Cross_Cursor(x,y);
Tek_Char_PosSet(x,y);
spi_puts("Color TEST\n\r"); //メッセージを表示

spi_puts("\033[39m"); //前景をデフォルト色に戻す
spi_puts("\033[49m"); //背景をデフォルト色に戻す
x=30;
y=size_y-40;
Tek_Cross_Cursor(x,y);
Tek_Char_PosSet(x,y);
spi_puts("Tek4010 Graphic Test For Tera-Term!\n\r"); //メッセージを表示

Exit_Tek4010(); //Tekウィンドウを終了する(通常のTeraTermウィンドウに戻す)

spi_puts("This message is displayed in main window of TeraTerm!!\n\r"); //このメッセージはTEKウィンドウではなく,通常のTeraTermウィンドウに表示

}

パラレルリンクの動力学

今週末は2軸プロッタの動力学を組み込むべく,PC側で数値解析ソフトを作成中.

2軸プロッタのモデルを下図に示す.




2個のモータでリンク1,およびリンク2を駆動する平行リンク型の構造.
多リンクロボットの運動方程式は一般に以下のように書ける.

ここで,M(θ)は慣性項,h(θ,θ')はコリオリ力,遠心力等の非線形項,g(θ)は重力項である.
τはトルクを表し,上記の式が求まればロボットを駆動するに必要な関節トルクを求めることができる.

今回の2軸ロボットでは最初の部分の慣性項Mは以下の2x2行列となる.

見た目ゴッチャリしているが,よく見ると行列の対角成分は似たような形をしている.
この式にはおもしろい特性がある.


上記赤枠の部分が各軸が独立して持っているイナーシャであり,ここにはsinやcosが付いてないことから定数である.
そして青枠の部分が多軸が相互に干渉する部分であり,ここはアーム角度の関数になっていることがわかる.
そして良く見ると,青枠の2つの式はまったく同じである.
ここで,青枠の式のcosの前の係数に着目し,以下が成り立つようにリンクの寸法や重量を調節したとする.

するとcosの前の項はゼロとなり,青枠項がゼロとなる.
このとき,ロボットは下図のように各軸が完全に独立したとみなせる.




またコリオリ力や遠心力などの非線形項は,次の式で求まる.

偏微分式なのでとっつきにくいが,要は慣性行列Mから上の式を使えばh(q,q')も求まる,ということを意味している.
(あるいはロボットの運動エネルギTがすでに求まっていれば,それを使っても良い)
実際に計算すると以下を得る.


ここでも慣性行列の青枠式と同じ係数(下線部)が出てきている.
よって係数をゼロにする条件を満たすと,h(q,q')=0となり,ロボットは非線形性を持たない独立した1慣性モデルと等価になる.
なお,鉛直方向に運動させるときは,重力項はゼロにはできない.

このように機械的に非線形項をゼロに調節できるのは平行リンク型ならではの特徴であり,
たとえば下図のような2リンク直列型マニピュレータではリンクの長さや重量をいかに選んでも非線形項はゼロにできない.


直列型では固定された根元から見た場合,駆動部1の先に駆動部2が付いているので駆動部2が動けば当然駆動部1にも力やモーメントが伝わってしまう.
一方でパラレル型は2つの駆動の幾何学的合力で動くため,うまいことやればこれらを切り離すことができる.
このような特性は逆動力学計算の負荷を大幅に減らせるため,使わない手は無い.
≪PREVNEXT≫
プロフィール

もやね

Author:もやね
長野県在住の会社員(メカニカル・エンジニア).
ロボットは完全な趣味としてやってます.
E-mail:
mo_ya_ne[a]yahoo.co.jp
[a]⇒@

最近の記事
最近のコメント
最近のトラックバック
月別アーカイブ
カテゴリー
FC2カウンター
ブログ内検索
RSSフィード
リンク
上記広告は1ヶ月以上更新のないブログに表示されています。新しい記事を書くことで広告を消せます。