MATLABでの直動型倒立振子のシンプルなLQR制御例を示します。
あなたのコードは状態空間やLQR設計、制御入力の計算が混乱しているので、整理したものを提示します。matlab
% --- パラメータ ---
M = 4.4; m = 0.1; l = 0.115; g = 9.81; b = 5; o = 0.006; J = m*l^2/3;
Ts = 0.001; T = 10; N = round(T/Ts)+1; t = (0:N-1)*Ts;
% --- 初期状態 [位置; 速度; 角度; 角速度] ---
x = [0; 0; pi + 1*pi/180; 0]; % 小さく傾けて開始
XX = zeros(4,N); XX(:,1) = x;
U = zeros(1,N);
% --- 状態空間線形化（倒立振子線形モデル） ---
A = [0 1 0 0;
0 -b/(M+m) m*g/(M+m) 0;
0 0 0 1;
0 -b*l/(J+m*l^2) m*g*l/(J+m*l^2) -o/(J+m*l^2)];
B = [0; 1/(M+m); 0; l/(J+m*l^2)];
Q = diag([10,1,100,1]);
R = 0.01;
% --- LQRゲイン計算 ---
F = lqr(A,B,Q,R);
% --- 動的シミュレーション（非線形モデル） ---
for k = 1:N-1
th = x(3); thd = x(4);
x_lin = x;
x_lin(3) = wrapToPi(th); % 角度を[-pi,pi]に正規化
u = -F*x_lin;
U(k) = u;
% --- 非線形RK4 ---
f = @(x,u) [
x(2);
(u - b*x(2) + m*l*x(4)^2*sin(x(3)))/(M+m);
x(4);
(m*g*l*sin(x(3)) - o*x(4) + u*l - m*l*x(4)^2*sin(x(3))*l)/(J+m*l^2)
];
k1 = f(x,u);
k2 = f(x + Ts/2*k1,u);
k3 = f(x + Ts/2*k2,u);
k4 = f(x + Ts*k3,u);
x = x + Ts/6*(k1 + 2*k2 + 2*k3 + k4);
XX(:,k+1) = x;
end
% --- グラフ表示 ---
figure('Color','w');
subplot(3,1,1)
plot(t, wrapToPi(XX(3,:))*180/pi, 'LineWidth',1)
ylabel('\\theta [deg]'); grid on; title('振子の角度')
subplot(3,1,2)
plot(t, U, 'LineWidth',1)
ylabel('u [N]'); grid on; title('入力')
subplot(3,1,3)
plot(t, XX(1,:), 'LineWidth',1)
yline(0.75,'k--'); yline(-0.75,'k--');
ylabel('z [m]'); xlabel('時間 [s]'); grid on; title('カート位置')
ポイント：
* LQR設計は倒立振子線形化モデルで計算
* 非線形シミュレーションはRK4で更新
* 角度は wrapToPi で [-pi,pi] に正規化
* 状態フィードバックは
