FC2ブログ

爪車

ヒト型ロボット 56 ZMP規範の歩行パターン生成に着手

今までは、ロボットの歩行パターンの生成に3次元倒立振子規範の方法を使用していましたが、ZMP規範の方法も試してみることにしました。

ZMPを規範とするパターン生成法には複数の手法があるようなのですが、今回試してみたのは「ヒューマノイドロボット」(オーム社)のP144, 4.4.3 オンラインパターン生成 で紹介されている予見制御による歩行パターンの生成です。

本を何度読んでも、さっぱり理論が理解できないでいるのですが、Octaveで試行錯誤しながら実装してみたところ、それっぽい結果が出てきました。


縦軸:距離[m], 横軸:時間[s], 緑線:目標ZMP, 赤線:計算によるZMP, 青線:重心軌道

サンプリング時間はけっこう細かくしないといけないみたいです。
今回のは0.001で試しています。
0.01で試したところ、ZMPの軌道が目標ZMPに追従しきれず、なまってしまいました。

サンプリング時間が細かいので、繰り返し計算の回数がかなり多くなります。
下手に実装すると計算に時間が掛かりますが、事前に計算しておける部分が多いので、上手く実装すれば計算時間は少なくてすみます。

« ヒト型ロボット 57 C++に移植|Top|ヒト型ロボット 55 歩行しませんでした »

コメント

Y軸方向がうまく出来ません

初めまして

ZMP規範の歩行パターンを作っています。

X軸方向は本と同じようなグラフを出せたのですが

Y軸方向がうまく出来ません
(ZMPを目標ZMPに追従させると、重心の振動が大きくなり、
重心の振動を抑えると、ZMPが目標ZMPのエッジ以外で外れてしまいます)

最適レギュレータの重み(Q,R)は、どのような値で設定していますか?

Re: Y軸方向がうまく出来ません

> 初めまして
>
> ZMP規範の歩行パターンを作っています。
>
> X軸方向は本と同じようなグラフを出せたのですが
>
> Y軸方向がうまく出来ません
> (ZMPを目標ZMPに追従させると、重心の振動が大きくなり、
> 重心の振動を抑えると、ZMPが目標ZMPのエッジ以外で外れてしまいます)
>
> 最適レギュレータの重み(Q,R)は、どのような値で設定していますか?

Qは各要素が100の対角行列。
Rは1にしていました。

ご参考までに、当時書いたoctave用のソースコードです。

% licence: CC0 (https://creativecommons.org/publicdomain/zero/1.0/deed.ja)
% y軸方向
clear;
disp("-- TCM y --\n");

Zc = 0.8;
g = 9.8;
dT = 0.002;
Ad = [1 dT (dT^2)/2; 0 1 dT; 0 0 1];
Bd = [(dT^3)/6; (dT^2)/2; dT];
Cd = [1 0 (-Zc/g)];

As = [1 -Cd*Ad; zeros(3, 1) Ad];
G = [-Cd*Bd; Bd];
Gr = [1; 0; 0; 0];

%Calculation of optimal state feedback gain
Qq = [100];
R = [1];
Q = eye(4)*Qq;
[K P E] = dlqr(As, G, Q, R);
disp("Optimal feedback gain");
disp(K);
disp("P");
disp(P);

K = -(inv(R + (G')*P*G))*(G')*P*As;
disp("Kd");
disp(K);

Ks = K(1);
Kx = [K(2) K(3) K(4)];

Ep = (eye(4) - G*(inv(R + (G')*P*G))*(G')*P)*As;

% 予測制御ゲイン
Tf = 1.6;
Mr = Tf/dT;

for i = 1:Mr
fd(i) = -(inv(R +( G')*P*G))*(G')*((Ep')^(i - 1))*P*Gr;
tf(i) = i*dT;
endfor

%plot(tf, fd);
%pause

% pref の作成
Tsup = 1.0;
n = Tsup/dT;
sx = 0.2;
sn = 0;
ic = 0;
cnt = 0;
for i = 1:20*n
pr(i) = sx*((-1)^sn);
time(i) = i*dT;
if (ic == n)
if (cnt > 1)
if ( cnt > 7)
else
sn = sn + 1;
endif
endif
cnt = cnt + 1;
ic = 0;
endif
ic = ic + 1;
endfor

% simulation part
dt = dT;
fp = 0;
ic = 0;
cnt = 0;
x = [0.2; 0; 0];
%u = -K*x + fp;

for i = 1:10*n
fp = 0;
for j = 1:Mr
fp = fp + fd(j)*pr(i+j);
endfor
u = Kx*x + fp;
xdot = Ad*x + Bd*u;

zmp = Cd*x;
y1(i) = x(1);
y2(i) = x(2);
tpr(i) = pr(i);
sp(i) = zmp;
ts(i) = i*dt;
um(i) = u;
x = xdot;

endfor
K
fp
plot(ts, y1, ts, tpr, ts, sp);
%plot(ts, y2);
pause
disp("-- END --\n");

返答ありがとうございます

ソース
参考にさせてもらいます

コメントの投稿

管理者にだけ表示を許可する

トラックバック

http://tsumeguruma.blog46.fc2.com/tb.php/126-db17e0b5

Top

HOME

tsumehashi

Author:tsumehashi
FC2ブログへようこそ!

07 | 2018/08 | 09
- - - 1 2 3 4
5 6 7 8 9 10 11
12 13 14 15 16 17 18
19 20 21 22 23 24 25
26 27 28 29 30 31 -