プリシェーピング法による制振制御

力学

荷物や台車を運搬するときに加速度を与える、与え方を間違えると共振してしまうか、揺れが大きくなってしまう。今回は揺れを抑えるプリシェーピング法を記述する.ここではモデルについてはざっくり書き、どういった制振制御なのか、実験動画とシミュレーションで比較したりする。

モデル、制御対象

次に示す図１のようなクレーンのモデルを考える。

図１　クレーンモデル

ここで、台車の質量を$M$, 荷物の質量を$m$, 台車の原点からの距離を$x$, 荷物の振れ角を$\theta$,回転軸における摩擦減衰係数を$c$とする。


サーボシステムを用いるとクレーンは次のような状態空間表現で表せる。

\begin{gather}
\begin{bmatrix}
\dot{x}\\
\ddot{x}\\
\dot{\theta}\\
\ddot{\theta}
\end{bmatrix}=
\begin{bmatrix}
0&1&0&0\\
0&0&0&0\\
0&0&0&1\\
0&0&-\frac{g}{l}&-\frac{c}{ml^2}
\end{bmatrix}
\begin{bmatrix}
x\\
\dot{x}\\
\theta\\
\dot{\theta}
\end{bmatrix}+
\begin{bmatrix}
0\\
1\\
0\\
-\frac{1}{l}
\end{bmatrix}a
\tag{1}
\end{gather}

ここで$a$は、目標加速度を示す。

補足

この振り子と台車のモデルは次のように示せる。
\begin{gather}
ml^2\ddot{\theta} = -mgl\sin \theta -m\ddot{x}l\cos \theta -c \dot{\theta}
\end{gather}
\begin{gather}
M\ddot{x} = F-m(\ddot{x}+l\ddot{\theta}\cos \theta -l\dot{\theta}^2 \sin \theta)
\end{gather}
これらを線形近似すると
\begin{gather}
\ddot{\theta} = -\frac{g}{l}\theta -\frac{1}{l}\ddot{x}-\frac{c}{ml^2} \dot{\theta}\\
\ddot{x} = \frac{F}{M+m}-\frac{ml}{M+m}\ddot{\theta} \tag{2}
\end{gather}
(2)式の右辺の第２項は、振り子の水平方向移動が台車の挙動に影響を与えていることを示しており、この項は外乱として扱うことができる。そのため次のように表せる
\begin{gather}
\ddot{x}=\frac{F}{M+m}+d
\end{gather}

ブロック線図で表すと図2のようになる。

図２　位置制御システムのブロック線図

ここでサーボモーターの位置制御によって変位がフィードバックされるため、外乱項のdは無視できる。つまり$x_r\approx x$が言える。言い換えると、入力位置がそのまま出力される。

制振制御なし

シミュレーション

質量やロープ長、等価粘性減衰係数などのパラメータや台車装置の仕様を以下のように定義する。

・質量$m=1.0$Kg
・ロープ長$l=1.2$m
・等価粘性減衰係数 $c=0.001$

・サンプリング時間$dt=0.001$s
・シミュレーション時間$Tsim=20$s

・加速度制限$a_{lim}=1$m/ss
・最大速度$v_{max}=0.4$m/s
・搬送距離$x_{rr}=0.7$m
この条件でoctave(matlab)を用いてシミュレーションを行う


clear
close all
%% =========== 設定パラメータ ============

Tsim = 20; %シミュレーション時間 [s]
sampleT = 0.001; %サンプリング時間 [s]
g = 9.8; %重力加速度 [m/s^2]
l = 1.2;   %ロープ(棒)長さ[m]
m = 1;   %重さ[kg]
wn = sqrt(g/l);   %固有振動数[rad/s]
c = 0.001; %等価粘性減衰係数
z = c/(2*m*l*wn);   %減衰比

%% =========== 装置仕様 =================

Alim = 1; %加速度制限 [m/s^2]
Vmax = 0.4; %最大速度 [m/s]
Xrr = 0.7; %搬送距離 [m]

%搬送開始前停止時間 [s]
Ts = 1.0;
%加速・減速区間の時間
Ta = Vmax/Alim;
%%等速区間の時間
Tv = Xrr/Vmax - Vmax/Alim;



%初期停止の
Ns = floor(Ts / sampleT)+1;%1001個
Na = floor(Ta/sampleT);%400個
Nv = floor(Tv/sampleT);%1349個
%減速後の停止
Nf = round(Tsim/sampleT) + 1 - (Ns+2*Na+Nv);%16851個


a_s = zeros(1,Ns);%初期停止
a_a = Alim*ones(1,Na);%加速または減速
a_v = zeros(1,Nv);%等速
a_f = zeros(1,Nf);%最後のあまり

%加速パターン
a = [a_s a_a a_v -a_a a_f];

T = (0:sampleT:Tsim);


%微分方程式の解
A = [0 1 0 0; 0 0 0 0; 0 0 0 1; 0 0 -g/l -c/(m*l^2)];
B = [0; 1; 0; -1/l];

u1 = a;

x(:,1)=[0 0 0 0];   %xの初期値

for i = 1:round(Tsim/sampleT)+1
    x(:,i+1) = x(:,i)+sampleT*(A*x(:,i)+B*u1(i));   %状態方程式
    y(:,i) = x(:,i);   %出力方程式
end




%グラフ作成
figure
subplot(4,1,1,'fontsize',20)
FigU=plot(T,u1);grid;
set(FigU,'linewidth',2)
ylabel('Acc.[m/s^2]','fontsize',20);
set(gca,'linewidth',1,'fontsize',20)

subplot(4,1,2)
Fig2=plot(T,y(2,:));grid;
set(Fig2,'linewidth',2)
ylabel('Vel.[m/s]','fontsize',20);
set(gca,'linewidth',1,'fontsize',20)


subplot(4,1,3)
Fig3=plot(T,y(1,:));grid;
set(Fig3,'linewidth',2)
ylabel('Position [m]','fontsize',20);
set(gca,'linewidth',1,'fontsize',20)


subplot(4,1,4)
Fig3 = plot(T,y(3,:)*180/pi);grid;
set(Fig3,'linewidth',2)
ylabel('angle [deg]','fontsize',20);
xlabel('Time [s]','fontsize',20);
set(gca,'linewidth',1,'fontsize',20)

%csvwrite('temp.csv', y_a(2,:));

図3 シミュレーション結果

グラフの結果は上から、台車の加速度、台車の速度、台車の変位、そして荷物の振れ角である。
目標位置についてからマイナスの加速度を入力した場合、台車の変位は目標位置で止まりますが荷物は振動したままになっている。

実験動画

研究室によるクレーンで実験を行った動画を下に示す。

プリシェーピング法

プリシェーピング法は、図４のように、波の半周期後に同じ加速度を与えることで振動を抑制する制御手法である。

図4 プリシェーピング法の概要

シミュレーション

プログラムにする際のコツは半周期分の0ベクトルを生成すること。

 clear 
close all


%% =========== 設定パラメータ ============

Tsim = 20; %シミュレーション時間 [s]
sampleT = 0.001; %サンプリング時間 [s]
g = 9.8; %重力加速度 [m/s^2]
l = 1.2;   %ロープ(棒)長さ[m]
m = 1;   %重さ[kg]
wn = sqrt(g/l);   %固有振動数[rad/s]
c = 0.001; %等価粘性減衰係数
z = c/(2*m*l*wn);   %減衰比


%システム行列
A = [0 1 0 0; 0 0 0 0; 0 0 0 1; 0 0 -g/l -c/(m*l^2)];
B = [0; 1; 0; -1/l];

T_koyuu = 2*pi/wn;
t_half = T_koyuu*0.5; %1.73s
T_half= round(t_half/sampleT);

%% =========== 装置仕様 =================

Alim = 1; %加速度制限 [m/s^2]
Vmax = 0.4; %最大速度 [m/s]
Xrr = 0.7; %搬送距離 [m]
%% =========== 停止時間 =================
Ts = 1.0; %搬送開始前停止時間 [s]



%加速・減速区間の時間
Ta = Vmax/Alim;
%%等速区間の時間
Tv = Xrr/Vmax - Vmax/Alim;

%初期停止、何個要素が必要か
Ns = floor(Ts / sampleT)+1;
%加速または減速
Na = floor(Ta/sampleT);
%等速
Nv = floor(Tv/sampleT);
%Tsimが変わっても実行できるように最後ところで数を合わせる
Nf = round(Tsim/sampleT) + 1 - (Ns+2*Na+Nv+T_half);

a_s = zeros(1,Ns);%初期停止
a_a = Alim*ones(1,Na);%加速または減速
a_v = zeros(1,Nv);%等速
a_f = zeros(1,Nf);%最後のあまり
a4 = 0*ones(1,T_half); %174個

%半周期分ずらして加速度を生成
a_1 = [a_s a_a a_v -a_a a4 a_f];%a_fを最後に加えているのは制振制御をわかりやすくするため。
a_2 = [a_s a4 a_a a_v -a_a a_f ];
a_sum = a_1+a_2;
%値が倍になってしまうため半分に要素を割る。制限を超える可能性がある。
a_sum =a_sum/2;

T = (0:sampleT:Tsim);
Tn = round(Tsim/sampleT);

%初期値
x_0=0;
v_0=0;
theta_0=0;
theta_a0=0;
x(:,1)=[x_0, v_0, theta_0, theta_a0];  

 for k = 1: Tn
    p1 = A*x(:,k)+B*a_sum(k);
    p2 = A*(x(:,k)+sampleT/2*p1)+B*a_sum(k);
    p3 = A*(x(:,k)+sampleT/2*p2)+B*a_sum(k);
    p4 = A*(x(:,k)+sampleT*p3)+B*a_sum(k);

    x(:,k+1) = x(:,k)+sampleT/6*(p1+2*p2+2*p3+p4);
 ans_d=x(1,:); 
 ans_v=x(2,:); 
 ans_theta=x(3,:); 
 ans_theta_v=x(4,:); 
 
  endfor
 


%台車の加速度
subplot(411);
plot(T,a_sum,'linewidth',2);
set(gca,'fontsize',20);
xlabel('time[s]','fontsize',20);
ylabel('a[m/s/s]','fontsize',20);
grid on;

%台車の速度
subplot(412);
plot(T,ans_v,'linewidth',2);
set(gca,'fontsize',20);
xlabel('time[s]','fontsize',20);
ylabel('v[m/s]','fontsize',20);
grid on;

%台車の距離
subplot(413);
plot(T,ans_d,'linewidth',2);
set(gca,'fontsize',20);
xlabel('time[s]','fontsize',20);
ylabel('x[m]','fontsize',20);
grid on;

%荷物の振れ角
subplot(414);
plot(T,ans_theta*180/pi,'linewidth',2);
set(gca,'fontsize',20);
xlabel('time[s]','fontsize',20);
ylabel('theta[deg]','fontsize',20);
grid on;

結果のグラフは上から、台車の加速度、台車の速度、台車の変位、そして荷物の振れ角を示す。

実験動画

プリシェーピング法を実装したクレーンの動画を以下に示す。

参考文献

[1]Dynamics & Design Conference, 星 龍貴ら,  2017

コメント

タイトルとURLをコピーしました