% --- main1.m ---
% 此為主執行檔。
clear; clc; close all;
% === 公共參數設定 ===
baseParams.Mp = 0.4753;
baseParams.g = 9.81;
baseParams.dm = 0.131;
baseParams.J = 0.0103;
baseParams.Jw = 0.0013;
baseParams.b_theta = 5.6799e-6;
baseParams.combined_b_alpha_term = 0.0131;
baseParams.combined_km_Ra_term = 0.0447;
baseParams.Ra = 2;
baseParams.ke = 0.1;
baseParams.km = baseParams.combined_km_Ra_term * baseParams.Ra;
baseParams.b_alpha = baseParams.combined_b_alpha_term - ...
(baseParams.km * baseParams.ke) / baseParams.Ra;
baseParams.tau_d = 0;
baseParams.alpha_ref = 0;
baseParams.u_max = 12;
baseParams.u_min = -12;
time_start = 0;
time_end = 0.5;
test_angle = 0.2;
% ========== 模擬 1:內迴路 PID 控制 ==========
params1 = baseParams;
params1.integral_error = 0;
params1.alpha_ref = 0;
params1.Kp_in = 10;
params1.Ki_in = 5;
params1.Kd_in = 3;
x0_1 = [test_angle; 0; 0; 0; 0]; % [theta; theta_dot; alpha; alpha_dot; E_in]
tspan1 = [time_start time_end];
options = odeset('RelTol', 1e-12, 'AbsTol', 1e-12);
[t1, x1] = ode45(@(t, x) dynamics(t, x, params1), tspan1, x0_1,options);
u1 = zeros(size(t1)); theta_ref1 = zeros(size(t1));
for i = 1:length(t1)
[~, u, theta_ref] = dynamics(t1(i), x1(i,:)', params1);
u1(i) = u; theta_ref1(i) = theta_ref;
end
% --- fuzzy_controller2.m ---
function theta_ref = fuzzy_controller2(e, de)
% === 範圍定義 ===
e_min = -50; e_max = 50;
de_min = -50; de_max = 50;
centers = [-10, -5, 0, 5, 10];
% === 定義三角形隸屬函數 ===
mf_e = define_mf(e_min, e_max, centers);
mf_de = define_mf(de_min, de_max, centers);
fprintf("e = %.4f\n", e);
fprintf("de = %.4f\n", de);
% === 模糊化 ===
mu_e = fuzzify(e, mf_e);
mu_de = fuzzify(de, mf_de);
rule_matrix = [
1 1 2 3 4
1 2 3 3 5
2 3 3 3 2
5 3 3 2 1
3 3 2 1 1
];
% === 推論輸出權重 ===
output_strength = zeros(1, 5);
for i = 1:5
for j = 1:5
activation = min(mu_e(i), mu_de(j));
k = rule_matrix(i, j);
output_strength(k) = max(output_strength(k), activation);
end
end
% === 解模糊===
numerator = sum(output_strength .* centers);
denominator = sum(output_strength);
theta_ref = 1 * (numerator / denominator);
end
% 定義隸屬度函數 (三角形)
function mf = define_mf(min_val, max_val, centers)
n = length(centers);
mf = cell(1, n);
for i = 1:n
if i == 1
a = min_val;
else
a = centers(i - 1);
end
b = centers(i);
if i == n
c = max_val;
else
c = centers(i + 1);
end
mf{i} = [a, b, c];
end
end
% 模糊化函數
function mu = fuzzify(value, mf)
n = length(mf);
mu = zeros(1, n);
for i = 1:n
a = mf{i}(1); b = mf{i}(2); c = mf{i}(3);
if value <= a || value >= c
mu(i) = 0;
elseif value >= a && value <= b
mu(i) = (value - a) / (b - a);
else
mu(i) = (c - value) / (c - b);
end
end
end
function [dxdt, u, theta_ref] = dynamics(~, x, params)
% 解包狀態
theta = x(1);
theta_dot = x(2);
alpha = x(3);
alpha_dot = x(4);
E_in = x(5); % 內迴路 PID 積分
% === 外迴路模糊控制器 ===
error_out = params.alpha_ref - alpha;
error_dot_out = -alpha_dot;
fprintf("error_out = %.4f\n", error_out);
fprintf("error_dot_out = %.4f\n", error_dot_out);
theta_ref = fuzzy_controller2(error_out, error_dot_out);
fprintf("theta = %.4f\n", theta);
fprintf("theta_ref = %.4f\n", theta_ref);
fprintf("alpha = %.4f\n", alpha);
% === 內迴路 PID 控制器 ===
error_in_pid = theta_ref - theta;
error_dot_in = -theta_dot;
fprintf("error_in_pid = %.4f\n", error_in_pid);
fprintf("error_dot_in = %.4f\n--------\n", error_dot_in);
u = params.Kp_in * error_in_pid + params.Ki_in * E_in + params.Kd_in * error_dot_in;
u = max(min(u, params.u_max), params.u_min); % 飽和限制
fprintf("u = %.4f\n", u);
% === 抗積分飽和 ===
if u >= params.u_max || u <= params.u_min
dE_in_dt = 0;
else
dE_in_dt = error_in_pid;
end
% === 動態模型 ===
Mp = params.Mp; g = params.g; dm = params.dm;
J = params.J; Jw = params.Jw;
b_theta = params.b_theta;
b_alpha_term = params.combined_b_alpha_term;
km_Ra = params.combined_km_Ra_term;
tau_d = params.tau_d;
theta_ddot = ((Mp * dm * g) / J) * sin(theta) - ...
(b_theta / J) * theta_dot + ...
(1 / J) * b_alpha_term * alpha_dot + ...
(1 / J) * tau_d - ...
(km_Ra / J) * u;
alpha_ddot = -((Mp * g * dm) / J) * sin(theta) + ...
(b_theta / J) * theta_dot - ...
((J + Jw) / (J * Jw)) * b_alpha_term * alpha_dot - ...
(1 / J) * tau_d + ...
((J + Jw) / (J * Jw)) * km_Ra * u;
dxdt = [theta_dot;
theta_ddot;
alpha_dot;
alpha_ddot;
dE_in_dt;];
end
--------
u = -12.0000
error_out = 16.0302
error_dot_out = 38.4436
e = 16.0302
de = 38.4436
theta = 4.8086
theta_ref = -10.0000
alpha = -16.0302
error_in_pid = -14.8086
error_dot_in = -10.9737
--------
u = -12.0000
error_out = 16.0502
error_dot_out = 38.4278
e = 16.0502
de = 38.4278
theta = 4.8143
theta_ref = -10.0000
alpha = -16.0502
error_in_pid = -14.8143
error_dot_in = -10.9447
--------
u = -12.0000
error_out = 16.0701
error_dot_out = 38.4120
e = 16.0701
de = 38.4120
theta = 4.8199
theta_ref = -10.0000
alpha = -16.0701
error_in_pid = -14.8199
error_dot_in = -10.9157
--------
u = -12.0000
error_out = 16.0901
error_dot_out = 38.3964
e = 16.0901
de = 38.3964
theta = 4.8256
theta_ref = -10.0000
alpha = -16.0901
error_in_pid = -14.8256
error_dot_in = -10.8868
--------
u = -12.0000
error_out = 16.1100
error_dot_out = 38.3809
e = 16.1100
de = 38.3809
theta = 4.8313
theta_ref = -10.0000
alpha = -16.1100
error_in_pid = -14.8313
error_dot_in = -10.8578
--------
u = -12.0000
error_out = 16.1296
error_dot_out = 38.3657
e = 16.1296
de = 38.3657
theta = 4.8368
theta_ref = -10.0000
alpha = -16.1296
error_in_pid = -14.8368
error_dot_in = -10.8294
--------
u = -12.0000
error_out = 16.1493
error_dot_out = 38.3506
e = 16.1493
de = 38.3506
theta = 4.8423
theta_ref = -10.0000
alpha = -16.1493
error_in_pid = -14.8423
error_dot_in = -10.8010
--------
u = -12.0000
error_out = 16.1689
error_dot_out = 38.3357
e = 16.1689
de = 38.3357
theta = 4.8479
theta_ref = -10.0000
alpha = -16.1689
error_in_pid = -14.8479
error_dot_in = -10.7726
--------
u = -12.0000
error_out = 16.1885
error_dot_out = 38.3208
e = 16.1885
de = 38.3208
theta = 4.8534
theta_ref = -10.0000
alpha = -16.1885
error_in_pid = -14.8534
error_dot_in = -10.7443
--------
u = -12.0000
error_out = 16.2078
error_dot_out = 38.3063
e = 16.2078
de = 38.3063
theta = 4.8588
theta_ref = -10.0000
alpha = -16.2078
error_in_pid = -14.8588
error_dot_in = -10.7163
--------
u = -12.0000
error_out = 16.2271
error_dot_out = 38.2919
e = 16.2271
de = 38.2919
theta = 4.8642
theta_ref = -10.0000
alpha = -16.2271
error_in_pid = -14.8642
error_dot_in = -10.6885
--------
u = -12.0000
error_out = 16.2465
error_dot_out = 38.2775
e = 16.2465
de = 38.2775
theta = 4.8696
theta_ref = -10.0000
alpha = -16.2465
error_in_pid = -14.8696
error_dot_in = -10.6606
--------
u = -12.0000
error_out = 16.2658
error_dot_out = 38.2633
e = 16.2658
de = 38.2633
theta = 4.8749
theta_ref = -10.0000
alpha = -16.2658
error_in_pid = -14.8749
error_dot_in = -10.6328
--------
u = -12.0000
error_out = 16.2848
error_dot_out = 38.2494
e = 16.2848
de = 38.2494
theta = 4.8802
theta_ref = -10.0000
alpha = -16.2848
error_in_pid = -14.8802
error_dot_in = -10.6053
--------
u = -12.0000
error_out = 16.3039
error_dot_out = 38.2356
e = 16.3039
de = 38.2356
theta = 4.8855
theta_ref = -10.0000
alpha = -16.3039
error_in_pid = -14.8855
error_dot_in = -10.5779
--------
u = -12.0000
error_out = 16.3229
error_dot_out = 38.2218
e = 16.3229
de = 38.2218
theta = 4.8908
theta_ref = -10.0000
alpha = -16.3229
error_in_pid = -14.8908
error_dot_in = -10.5506
--------
u = -12.0000
error_out = 16.3420
error_dot_out = 38.2082
e = 16.3420
de = 38.2082
theta = 4.8960
theta_ref = -10.0000
alpha = -16.3420
error_in_pid = -14.8960
error_dot_in = -10.5232
--------
u = -12.0000
error_out = 16.3608
error_dot_out = 38.1948
e = 16.3608
de = 38.1948
theta = 4.9012
theta_ref = -10.0000
alpha = -16.3608
error_in_pid = -14.9012
error_dot_in = -10.4962
--------
u = -12.0000
error_out = 16.3796
error_dot_out = 38.1816
e = 16.3796
de = 38.1816
theta = 4.9064
theta_ref = -10.0000
alpha = -16.3796
error_in_pid = -14.9064
error_dot_in = -10.4693
--------
u = -12.0000
error_out = 16.3984
error_dot_out = 38.1684
e = 16.3984
de = 38.1684
theta = 4.9115
theta_ref = -10.0000
alpha = -16.3984
error_in_pid = -14.9115
error_dot_in = -10.4423
--------
u = -12.0000
error_out = 16.4172
error_dot_out = 38.1554
e = 16.4172
de = 38.1554
theta = 4.9166
theta_ref = -10.0000
alpha = -16.4172
error_in_pid = -14.9166
error_dot_in = -10.4155
--------
u = -12.0000
error_out = 16.4358
error_dot_out = 38.1425
e = 16.4358
de = 38.1425
theta = 4.9217
theta_ref = -10.0000
alpha = -16.4358
error_in_pid = -14.9217
error_dot_in = -10.3889
--------
u = -12.0000
error_out = 16.4544
error_dot_out = 38.1298
e = 16.4544
de = 38.1298
theta = 4.9268
theta_ref = -10.0000
alpha = -16.4544
error_in_pid = -14.9268
error_dot_in = -10.3624
--------
u = -12.0000
error_out = 16.4730
error_dot_out = 38.1172
e = 16.4730
de = 38.1172
theta = 4.9318
theta_ref = -10.0000
alpha = -16.4730
error_in_pid = -14.9318
error_dot_in = -10.3359
--------
u = -12.0000
error_out = 16.4916
error_dot_out = 38.1047
e = 16.4916
de = 38.1047
theta = 4.9369
theta_ref = -10.0000
alpha = -16.4916
error_in_pid = -14.9369
error_dot_in = -10.3094
--------
u = -12.0000
error_out = 16.5100
error_dot_out = 38.0924
e = 16.5100
de = 38.0924
theta = 4.9418
theta_ref = -10.0000
alpha = -16.5100
error_in_pid = -14.9418
error_dot_in = -10.2832
--------
u = -12.0000
error_out = 16.5284
error_dot_out = 38.0802
e = 16.5284
de = 38.0802
theta = 4.9468
theta_ref = -10.0000
alpha = -16.5284
error_in_pid = -14.9468
error_dot_in = -10.2571
--------
u = -12.0000
error_out = 16.5468
error_dot_out = 38.0681
e = 16.5468
de = 38.0681
theta = 4.9517
theta_ref = -10.0000
alpha = -16.5468
error_in_pid = -14.9517
error_dot_in = -10.2310
--------
u = -12.0000
error_out = 16.5652
error_dot_out = 38.0561
e = 16.5652
de = 38.0561
theta = 4.9567
theta_ref = -10.0000
alpha = -16.5652
error_in_pid = -14.9567
error_dot_in = -10.2050
--------
u = -12.0000
error_out = 16.5806
error_dot_out = 38.0461
e = 16.5806
de = 38.0461
theta = 4.9608
theta_ref = -10.0000
alpha = -16.5806
error_in_pid = -14.9608
error_dot_in = -10.1832
--------
u = -12.0000
error_out = 16.5960
error_dot_out = 38.0361
e = 16.5960
de = 38.0361
theta = 4.9649
theta_ref = -10.0000
alpha = -16.5960
error_in_pid = -14.9649
error_dot_in = -10.1614
--------
u = -12.0000
error_out = 16.6114
error_dot_out = 38.0263
e = 16.6114
de = 38.0263
theta = 4.9690
theta_ref = -10.0000
alpha = -16.6114
error_in_pid = -14.9690
error_dot_in = -10.1396
--------
u = -12.0000
error_out = 16.6268
error_dot_out = 38.0165
e = 16.6268
de = 38.0165
theta = 4.9731
theta_ref = -10.0000
alpha = -16.6268
error_in_pid = -14.9731
error_dot_in = -10.1179
--------
u = -12.0000
最新发布