爪車

ヒト型ロボット 35 両脚支持期間の導入

線形倒立振子の歩行パターンに「ヒューマノイドロボット」(オーム社)、4.3.4の両脚支持期間を導入してみました。

本の通りに両脚支持期間の重心の位置は時間に関する4次関数
x(t) = a0 + a1*t + a2*t^2 + a3*t^3 + a4*t^4
で求めました。

係数a0...a4は上の式と、上の式を1階、2階、3階まで微分してできた式と、支持脚交代前後の状態(位置、速度、加速度)、両脚支持期間の条件から連立方程式により求めます。

両脚支持期間導入前の重心のx方向の速度
(重心高さZc=0.8,歩行周期Tsup=0.8,x方向歩幅0.3, y方向歩幅0.2)
Screenshot-Gnuplot-dx.gif
支持脚の交代が一瞬で行われるので、支持脚の交代時に重心の速度は屈曲点になります。

両脚支持期間を導入後の重心のx方向の速度
(重心高さZc=0.8,歩行周期Tsup=0.7,両脚支持期間Tabl=0.1,x方向歩幅0.3, y方向歩幅0.2)
Screenshot-Gnuplot-dx-s.gif
青い部分が、両脚支持期間の導入による効果です。
両脚支持期間の導入により、重心の速度が、滑らかに変化するようになりました。


両脚支持期間導入前のxy平面の重心の軌跡
(重心高さZc=0.8,歩行周期Tsup=0.8,x方向歩幅0.3, y方向歩幅0.2)
Screenshot-Gnuplot-xy.gif

両脚支持期間導入後のxy平面の重心の軌跡
(重心高さZc=0.8,歩行周期Tsup=0.7,両脚支持期間Tabl=0.1,x方向歩幅0.3, y方向歩幅0.2)
Screenshot-Gnuplot-xy-s.gif
青い部分が両脚支持期間における重心の軌跡です。
両脚支持期間の導入により、歩幅が増大しています。




« ヒト型ロボット 36 歩行パターン生成ほぼ完成|Top|C言語でgnuplotを利用してグラフ表示 »

コメント

両脚支持期間に関して

初めまして。

古金と申します。

現在「ヒューマノイドロボット」を参考に

2足歩行ロボットを作成しております。

倒立振子を使用した歩行の実装を行っているのですが、

両脚支持期間の算出方法がわからずつまずいています。

何かアドバイスを頂けないでしょうか?

Re: 両脚支持期間に関して

古金さん。

初めまして。tsumehashiです。

両脚支持期間の算出ですが、「ヒューマノイドロボット」(4.61)式
x(t) = a0 + a1*t + a2*t^2 + a3*t^3 + a4*t^4
の係数a0...a4を求めるためには、まず(4.61)式
x(t) = a0 + a1*t + a2*t^2 + a3*t^3 + a4*t^4 ... (1)
を2階まで微分してx''(t)まで求めます。

1階微分
x'(t) = a1 + 2*a2*t + 3*a3*t^2 + 4*a4*t^3 ... (2)

2階微分
x''(t) = 2*a2 + 6*a3*t + 12*a4*t^2 ... (3)

求めた (1)、(2)、(3)の3つの式に
両脚支持期開始時 t = 0 のとき(支持脚交代前)の状態(位置、速度、加速度)と、
両脚支持期終了時 t = Tdbl のとき(支持脚交代後)の状態(位置、速度、加速度)
を代入して係数a0...a4を求めます。

係数a0...a4を求めるには、条件が5つあればよいので、今回は
両脚支持期開始時 t = 0 のときの位置 : x0、速度 : x0'、加速度 : x0''
両脚支持期終了時 t = Tdbl のときの位置 : x1、速度 : x1'
をもとにして求めます。

t = 0 のときの位置 x0 は、(1)式にt = 0を代入して、
x0 = x(t=0) = a0 ... (4)

t = 0 のときの速度 x0' は、(2)式に t = 0 を代入して、
x0' = x'(t=0) = a1 ... (5)

t = 0 のときの加速度 x0'' は、(3)式に t = 0 を代入して、
x0'' = x''(t=0) = 2*a2 . .. (6)

t = Tdbl のときの位置 x1 は、(1)式に t = Tdbl を代入して、
x1 = x(t=Tdbl) = a0 + a1*Tdb l+ a2*Tdbl^2 + a3*Tdbl^3 + a4*Tdbl^4 ... (7)

t = Tdbl のときの速度 x1' は、(2)式に t = Tdbl を代入して、
x1' = x'(t=Tdbl) = a1 + 2*a2*Tdbl + 3*a3*Tdbl^2 + 4*a4*Tdbl^3 ... (8)

(4)、(5)、(6)、(7)、(8)の5つの式を連立させれば、
係数a0、a1、a2、a3、a4を求めることができます。

係数を求めるのに必要な加速度ですが、
これは、「ヒューマノイドロボット」(4.53)式を微分して求めればよいです。
x''(t) = (xi(n) - px*) / (Tc^2) * cosh(t/Tc) + xi'(n) / Tc * sinh(t/Tc)

両脚支持期間に関して

tsumehashi さま

早速の回答ありがとうございます。

その様に考えるものなのですね。

教えて頂いた方法で一度算出してみたいと思います。

ご迷惑でなければ

また質問させて頂いてもよろしいでしょうか?

Re: 両脚支持期間に関して

ご質問やコメントを頂けると、こちらも嬉しいです。

両脚支持期間に関して

tsumehashi 様

お世話になっております。

両脚支持期間に関してなのですが、もう一度質問させて下さい。

軸足が切り替わるタイミングの位置、速度は計算では連続しているのですが、連立方程式を解く際に与えるパラメータは算出されたパラメータを与えるのでしょうか?

または、X方向の位置の変異量はこちらで任意で与えるものなのでしょうか?


たて続けにすいません。

両脚支持期間中は両脚を地面につけたまま重心を移動すると思うのですが、

次の一歩の初期重心位置は両脚支持期間が終了した時点での重心位置をパラメータとして与えるのでしょうか?

3つ連続での質問です。

こまごまと質問してすいません。

重心位置の4次関数はY方向に対しても行うのでしょうか?

Re: 両脚支持期間に関して

tsumehashiです。

両脚支持期間の計算ですが、先のコメントでの
>両脚支持期開始時 t = 0 のときの位置 : x0、速度 : x0'、加速度 : x0''
>両脚支持期終了時 t = Tdbl のときの位置 : x1、速度 : x1'
>をもとにして求めます。
は誤りで、正しくは
両脚支持期開始時 t = 0 のときの位置 : x0、速度 : x0'、加速度 : x0''
両脚支持期終了時 t = Tdbl のときの速度 : x1'、加速度 : x1''
となります。申し訳ございませんでした。

支持脚交代前後では、
速度、加速度が連続となり、位置はズレることになります。
両脚支持期間でズレた位置の分だけ、重心位置、着地位置も修正する必要があります。

また、y方向も同様に求めます。

tsumehashi 様

回答ありがとうございます。

計算では両脚支持期間の間に進むX方向の距離が分からず連立方程式で速度がなめらかに連続となるような距離を任意に入力していました。

両脚支持期間を考えない場合X方向の座標と速度は連続しており加速度は連続していませんでした。

この計算結果は加速度が連続していない時点で間違った結果と判断してよいのでしょうか?

Re: 両脚支持期間に関して

tsumehashiです。

先の私のコメント
>支持脚交代前後では、
>速度、加速度が連続となり、位置はズレることになります。
も誤りでした。

加速度は、そもそも不連続です(両脚支持期間挿入前は)。
不連続な加速度を滑らかにつなぐために、両脚支持期間を挿入します。

両脚支持期間Y方向

tsumehashi 様

お世話になります。

両足支持区間のY方向に関してなのですが、

「ヒューマノイドロボット」に記載されている

解析解のX成分の公式(4.52)(4.53)を全てYのパラメータに置き換えて速度、加速度を求めたのですが

正確な値が算出されていないように思います。

加速度に関しては1歩踏み出す間ずっと加速されいる様な曲線となり、速度に関しては最高速度からじょじょに減速されている様な曲線が算出されました。

ヒューマノイドロボットの公式をそのままY方向に適用出来ないのでしょうか?

Re: 両脚支持期間Y方向

古金 様

ご無沙汰しております。
tsumehashiです。

両脚支持期間を導入したプログラムです。
ご参考までに。

// Scilab用プログラム
clf();
clear();

Zc = 0.8; // 重心高さ
g = 9.8; // 重力加速度
Tsup = 0.8; //
Tdbl = 0.1; // 両脚支持期間
dT = 0.02;

n = 5; // 歩数

// Pattern 1
Sx = [0.0, 0.3, 0.3, 0.3, 0, 0, 0]; // n+2
Sy = [0.2, 0.2, 0.2, 0.2, 0.2, 0, 0];
//St = [0, 0, 0, 0, 0, 0, 0];

// Pattern 2
//Sx = [0.0, 0.3, 0.3, 0.3, 0, 0, 0];
//Sy = [0.2, 0.3, 0.1, 0.3, 0.2, 0, 0];
//St = [0, 0, 0, 0, 0, 0, 0];

// Pattern 3
//Sx = [0.0, 0.25, 0.25, 0.25, 0, 0, 0];
//Sy = [0.2, 0.2, 0.2, 0.2, 0.2, 0, 0];
//St = [0, 20, 40, 60, 60, 0, 0]; // (degree)

deg2rad = %pi / 180;

Tc = sqrt(Zc/g);
C = cosh(Tsup/Tc);
S = sinh(Tsup/Tc);

a = 10;
b = 1;
D = a*((C-1)^2) + b*((S/Tc)^2);

// Step. 1
x = 0.0; y = 0.01;
dx = 0; dy = 0;
ddx = 0; ddy = 0;

xi = x; yi = y;
dxi = dy; dyi = dy;
ddxi = ddy; ddyi = ddy;

px = 0.0; py = 0.0;
pxa = px; pya = py;

// Step. 2
T = 0;
time = 0;

plot2d(px, py, rect=[-0.2, -0.8, 1, 0.8]);
plot(px, py, ".r");

for i = 1:n+1

// Step. 3
for t=0:dT:Tsup
x = (xi-pxa)*cosh(t/Tc) + Tc*dxi*sinh(t/Tc) + pxa;
y = (yi-pya)*cosh(t/Tc) + Tc*dyi*sinh(t/Tc) + pya;
dx = (xi-pxa)/Tc*sinh(t/Tc) + dxi*cosh(t/Tc);
dy = (yi-pya)/Tc*sinh(t/Tc) + dyi*cosh(t/Tc);
ddx = (xi-pxa)/(Tc*Tc)*cosh(Tsup/Tc) + dxi/Tc*sinh(Tsup/Tc);
ddy = (yi-pya)/(Tc*Tc)*cosh(Tsup/Tc) + dyi/Tc*sinh(Tsup/Tc);

time = time + dT;

// plot(x,y, ".b"); // x y
plot(time,dx, ".b"); // dx
// plot(time,dy, ".b"); // dy
end

// Step. 4
T = T + Tsup;

xi = x;
yi = y;

dxi = dx;
dyi = dy;

ddxi = ddx;
ddyi = ddy;

// Step. 5
// (4.48)
px = px + Sx(i);
py = py - ((-1)^i) * Sy(i);

// (4.58)
// theta = deg2rad * St(i);
// px = px + cos(theta) * Sx(i) - sin(theta) * (- ((-1)^i) * Sy(i));
// py = py + sin(theta) * Sx(i) + cos(theta) * (- ((-1)^i) * Sy(i));

// Step. 6
// (4.49)
xb = Sx(i+1)/2;
yb = ((-1)^i)*Sy(i+1)/2;
// (4.50)
vxb = (C+1)/(Tc*S)*xb;
vyb = (C-1)/(Tc*S)*yb;

// // (4.59)
// theta = deg2rad * St(i+1);
// xb = cos(theta) * Sx(i+1)/2 - sin(theta) * ((-1)^i) * Sy(i+1)/2;
// yb = sin(theta) * Sx(i+1)/2 + cos(theta) * ((-1)^i) * Sy(i+1)/2;
//
// // (4.60)
// vxb = cos(theta) * (1+C)/(Tc*S)*xb - sin(theta) * (C-1)/(Tc*S)*yb;
// vyb = sin(theta) * (1+C)/(Tc*S)*xb + cos(theta) * (C-1)/(Tc*S)*yb;

// Step. 7
// (4.55)
xd = px + xb;
dxd = vxb;
yd = py + yb;
dyd = vyb;

// Step. 8
// (4.57)
pxa = -a*(C-1)/D*(xd - C*xi - Tc*S*dxi) - b*S/(Tc*D)*(dxd - S/Tc*xi - C*dxi);
pya = -a*(C-1)/D*(yd - C*yi - Tc*S*dyi) - b*S/(Tc*D)*(dyd - S/Tc*yi - C*dyi);

// 両脚支持期間の計算
xf = x; yf = y;
dxf = dx; dyf = dy;
ddxf = ddx; ddyf = ddy;

t = 0.0;
ddxfn = (xf-pxa)/(Tc*Tc)*cosh(t/Tc) + dxf/Tc*sinh(t/Tc);
ddyfn = (yf-pya)/(Tc*Tc)*cosh(t/Tc) + dyf/Tc*sinh(t/Tc);

ax1 = xf;
ax2 = dxf;
ax3 = ddxf/2;
ax4 = - (ddxfn + 2*ddxf)/(3*Tdbl);
ax5 = (ddxfn + ddxf)/(4*Tdbl*Tdbl);

ay1 = yf;
ay2 = dyf;
ay3 = ddyf/2;
ay4 = - (ddyfn + 2*ddyf)/(3*Tdbl);
ay5 = (ddyfn + ddyf)/(4*Tdbl*Tdbl);

for t=0:dT:Tdbl
x = ax1 + ax2*t + ax3*t^2 + ax4*t^3 + ax5*t^4;
y = ay1 + ay2*t + ay3*t^2 + ay4*t^3 + ay5*t^4;
dx = ax2 + 2*ax3*t + 3*ax4*t^2 + 4*ax5*t^3;
dy = ay2 + 2*ay3*t + 3*ay4*t^2 + 4*ay5*t^3;
ddx = 2*ax3 + 6*ax4*t + 12*ax5*t^2;
ddy = 2*ay3 + 6*ay4*t + 12*ay5*t^2;

time = time + dT;

// plot(x,y, ".r"); // x y
plot(time,dx, ".r"); // dx
// plot(time,dy, ".r"); // dy
end

tx = x - xf;
ty = y - yf;
px = px + tx;
py = py + ty;
pxa = pxa + tx;
pya = pya + ty;

T = T + Tsup;

xi = x;
yi = y;

dxi = dx;
dyi = dy;

ddxi = ddx;
ddyi = ddy;

printf("%i : px = %f, py = %f, pxa = %f, pya = %f\n", i, px, py, pxa, pya);

// plot(px, py, ".r"); // x y
// plot(pxa, pya, ".g"); // x y

end


コメントの投稿

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

トラックバック

http://tsumeguruma.blog46.fc2.com/tb.php/93-3371f836

Top

HOME

tsumehashi

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

08 | 2017/09 | 10
- - - - - 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