文章目錄
- 問題描述
- GPOPS代碼
- `main function`
- `continuous function`
- `endpoint function`
- 完整代碼
- 代碼仿真結果
- 最后
問題描述
例子出自論文 Direct solution of nonlinear optimal control problems using quasilinearization and Chebyshev polynomials(DOI:10.1016/S0016-0032(02)00028-5) Section 5.2. Example 2: Rigid asymmetric spacecraft
題目如下:
一個剛體非對稱航天器控制問題,其狀態方程為
{ ω ˙ 1 = ? I 3 ? I 2 I 1 ω 2 ω 3 + u 1 I 1 , ω ˙ 2 = ? I 1 ? I 3 I 2 ω 1 ω 3 + u 2 I 2 , ω ˙ 3 = ? I 2 ? I 1 I 3 ω 1 ω 2 + u 3 I 3 , (1) \left \{ \begin{matrix} \dot{\omega}_1 = -\frac{I_3-I_2}{I_1} \omega_2 \omega_3 + \frac{u_1}{I_1}, \\ \dot{\omega}_2 = -\frac{I_1-I_3}{I_2} \omega_1 \omega_3 + \frac{u_2}{I_2}, \\ \dot{\omega}_3 = -\frac{I_2-I_1}{I_3} \omega_1 \omega_2 + \frac{u_3}{I_3}, \end{matrix} \right. \tag{1} ? ? ??ω˙1?=?I1?I3??I2??ω2?ω3?+I1?u1??,ω˙2?=?I2?I1??I3??ω1?ω3?+I2?u2??,ω˙3?=?I3?I2??I1??ω1?ω2?+I3?u3??,?(1)
式中, ω 1 \omega_1 ω1?、 ω 2 \omega_2 ω2? 和 ω 3 \omega_3 ω3? 為航天器的角速度。
控制量為 u 1 u_1 u1?、 u 2 u_2 u2?、 u 3 u_3 u3?,目標函數為
J = 0.5 ∫ 0 100 ( u 1 2 + u 2 2 + u 3 2 ) d t . (2) J = 0.5 \int_{0}^{100}(u_1^2+u_2^2+u_3^2) \text{d}t. \tag{2} J=0.5∫0100?(u12?+u22?+u32?)dt.(2)
其余參數為
ω 1 ( 0 ) = 0.01 r/s , ω 1 ( t f ) = 0 r/s , ω 2 ( 0 ) = 0.005 r/s , ω 2 ( t f ) = 0 r/s , ω 3 ( 0 ) = 0.001 r/s , ω 3 ( t f ) = 0 r/s , I 1 = 86.24 kg?m 2 , I 2 = 85.07 kg?m 2 , I 3 = 113.59 kg?m 2 . (3) \begin{array}{lll} \omega_1(0)\ = 0.01\ \text{r/s} &, \ & \omega_1(t_f) = 0 \ \text{r/s}, \\ \omega_2(0)\ = 0.005\ \text{r/s} &, \ & \omega_2(t_f) = 0 \ \text{r/s}, \\ \omega_3(0)\ = 0.001\ \text{r/s} &, \ & \omega_3(t_f) = 0 \ \text{r/s}, \\ I_1=86.24 \ \text{kg m}^2 \ &,\ & I_2=85.07 \ \text{kg m}^2,\\ I_3=113.59 \ \text{kg m}^2. \end{array} \tag{3} ω1?(0)?=0.01?r/sω2?(0)?=0.005?r/sω3?(0)?=0.001?r/sI1?=86.24?kg?m2?I3?=113.59?kg?m2.?,?,?,?,??ω1?(tf?)=0?r/s,ω2?(tf?)=0?r/s,ω3?(tf?)=0?r/s,I2?=85.07?kg?m2,?(3)
GPOPS代碼
main function
首先設置GPOPS-II的參數。式 ( 3 ) (3) (3)給出了所有用到的參數,按照式 ( 3 ) (3) (3)寫出代碼即可。
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 功能描述:剛體非對稱航天器控制問題
% 文件名解釋:mainSpacecraftOCP.m 中,main 代表 主函數,
% Spacecraft 代表 航天器,
% OCP 代表 最優控制問題
% 作者:Lei Lie
% 時間:2024/06/22
% 版本:1.0
% - 完成了代碼框架的初始搭建
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clc;clear;close all;
tic;
%% 01.初始參數設置
%-------------------------------------------------------------------------%
%----------------------- 設置問題的求解邊界 ------------------------------%
%-------------------------------------------------------------------------%
% 設置時間
t0 = 0;
tf = 100;
% 設置狀態量初值
w10 = .01;
w20 = .005;
w30 = .001;
% 設置狀態量邊界條件
w1_max = 1;
w1_min = 0;
w2_max = 1;
w2_min = 0;
w3_max = 1;
w3_min = 0;
% 設置控制量初值
u10 = -.009;
u20 = -.004;
u30 = -.001;
% 設置控制量邊界條件
u1_max = 0;
u1_min = -.01;
u2_max = 0;
u2_min = -.01;
u3_max = 0;
u3_min = -.01;
% 設置靜態參數
auxdata.I1 = 86.24;
auxdata.I2 = 85.07;
auxdata.I3 = 113.59;
% 設置不等式約束邊界條件(路徑約束)
path_max = .002;
path_min = 0;%% 02.邊界條件設置
%-------------------------------------------------------------------------%
%------------------------ 將求解邊界設置于問題中 -------------------------%
%-------------------------------------------------------------------------%
bounds.phase.initialtime.lower = t0;
bounds.phase.initialtime.upper = t0;
bounds.phase.finaltime.lower = tf;
bounds.phase.finaltime.upper = tf;
bounds.phase.initialstate.lower = [w10 w20 w30];
bounds.phase.initialstate.upper = [w10 w20 w30];
bounds.phase.state.lower = [w1_min w2_min w3_min];
bounds.phase.state.upper = [w1_max w2_max w3_max];
bounds.phase.finalstate.lower = [0 0 0];
bounds.phase.finalstate.upper = [0 0 0];
bounds.phase.control.lower = [u1_min u2_min u3_min];
bounds.phase.control.upper = [u1_max u2_max u3_max];
bounds.phase.integral.lower = 0;
bounds.phase.integral.upper = 10000;%% 03.初值猜測
%-------------------------------------------------------------------------%
%------------------------------- 初值猜想 --------------------------------%
%-------------------------------------------------------------------------%
guess.phase.time = [t0; tf];
guess.phase.state = [[w10 w20 w30];[0 0 0]];
guess.phase.control = [[u10 u20 u30];[u10 u20 u30]];
guess.phase.integral = 100;%% 04.設置GPOPS求解器參數
%-------------------------------------------------------------------------%
%---------------------------- 設置求解器參數 -----------------------------%
%-------------------------------------------------------------------------%
setup.name = 'Spacecraft-OCP';
setup.functions.continuous = @socpContinuous;
setup.functions.endpoint = @socpEndpoint;
setup.bounds = bounds;
setup.guess = guess;
setup.auxdata = auxdata;
setup.nlp.solver = 'ipopt';
setup.derivatives.supplier = 'sparseCD';
setup.derivatives.derivativelevel = 'second';
setup.mesh.method = 'hp1';
setup.mesh.tolerance = 1e-6;
setup.mesh.maxiteration = 45;
setup.mesh.colpointsmax = 4;
setup.mesh.colpointsmin = 10;
setup.mesh.phase.fraction = 0.1*ones(1,10);
setup.mesh.phase.colpoints = 4*ones(1,10);
setup.method = 'RPMintegration';%% 05.求解
%-------------------------------------------------------------------------%
%----------------------- 使用 GPOPS2 求解最優控制問題 --------------------%
%-------------------------------------------------------------------------%
output = gpops2(setup);
solution = output.result.solution;
toc;
把結果畫出來,代碼如下。
%% 06.畫圖
t = solution.phase.time(:,1);
w1 = solution.phase.state(:,1);
w2 = solution.phase.state(:,2);
w3 = solution.phase.state(:,3);
u1 = solution.phase.control(:,1);
u2 = solution.phase.control(:,2);
u3 = solution.phase.control(:,3);figure('Color',[1,1,1]);
plot(t,w1,'-','LineWidth',1.5);hold on;
plot(t,w2,'-.','LineWidth',1.5);
plot(t,w3,'--','LineWidth',1.5);
xlabel('Time',...'FontWeight','bold');
ylabel('States',...'FontWeight','bold');
legend('w1','w2','w3',...'LineWidth',1,...'EdgeColor',[1,1,1],...'Orientation','horizontal',...'Position',[0.5,0.93,0.40,0.055]);
set(gca,'FontName','Times New Roman',...'FontSize',15,...'LineWidth',1.3);
print -dpng spacecraft_ocp_state.pngfigure('Color',[1,1,1]);
plot(t,u1,'-','LineWidth',1.5);hold on;
plot(t,u2,'-.','LineWidth',1.5);
plot(t,u3,'--','LineWidth',1.5);
xlabel('Time',...'FontWeight','bold');
ylabel('Control',...'FontWeight','bold');
legend('u1','u2','u3',...'LineWidth',1,...'EdgeColor',[1,1,1],...'Orientation','horizontal',...'Position',[0.5,0.93,0.40,0.055]);
set(gca,'FontName','Times New Roman',...'FontSize',15,...'LineWidth',1.3);
print -dpng spacecraft_ocp_control.png
continuous function
現在寫動力學方程,再把式 ( 1 ) (1) (1)重新寫一遍,放在這里。
{ ω ˙ 1 = ? I 3 ? I 2 I 1 ω 2 ω 3 + u 1 I 1 , ω ˙ 2 = ? I 1 ? I 3 I 2 ω 1 ω 3 + u 2 I 2 , ω ˙ 3 = ? I 2 ? I 1 I 3 ω 1 ω 2 + u 3 I 3 , \left \{ \begin{matrix} \dot{\omega}_1 = -\frac{I_3-I_2}{I_1} \omega_2 \omega_3 + \frac{u_1}{I_1}, \\ \dot{\omega}_2 = -\frac{I_1-I_3}{I_2} \omega_1 \omega_3 + \frac{u_2}{I_2}, \\ \dot{\omega}_3 = -\frac{I_2-I_1}{I_3} \omega_1 \omega_2 + \frac{u_3}{I_3}, \end{matrix} \right. ? ? ??ω˙1?=?I1?I3??I2??ω2?ω3?+I1?u1??,ω˙2?=?I2?I1??I3??ω1?ω3?+I2?u2??,ω˙3?=?I3?I2??I1??ω1?ω2?+I3?u3??,?
那么在代碼中可以這么寫:
dw1 = -((I3-I2)/I1) .* w2 .* w3 + u1 ./ I1;
dw2 = -((I1-I3)/I2) .* w1 .* w3 + u2 ./ I2;
dw3 = -((I2-I1)/I2) .* w1 .* w2 + u3 ./ I3;
性能指標的形式為
J = 0.5 ∫ 0 100 ( u 1 2 + u 2 2 + u 3 2 ) d t . J = 0.5 \int_{0}^{100}(u_1^2+u_2^2+u_3^2) \text{d}t. J=0.5∫0100?(u12?+u22?+u32?)dt.
對應的代碼如下。
phaseout.integrand = 0.5*(u1.^2 + u2.^2 + u3.^2);
那么,continues function
的完整代碼如下。
function phaseout = socpContinuous(input)w1 = input.phase.state(:,1);w2 = input.phase.state(:,2);w3 = input.phase.state(:,3);u1 = input.phase.control(:,1);u2 = input.phase.control(:,2);u3 = input.phase.control(:,3);I1 = input.auxdata.I1;I2 = input.auxdata.I2;I3 = input.auxdata.I3;dw1 = -((I3-I2)/I1) .* w2 .* w3 + u1 ./ I1;dw2 = -((I1-I3)/I2) .* w1 .* w3 + u2 ./ I2;dw3 = -((I2-I1)/I2) .* w1 .* w2 + u3 ./ I3;phaseout.dynamics = [dw1 dw2 dw3];phaseout.integrand = 0.5*(u1.^2 + u2.^2 + u3.^2);
end
endpoint function
此處代碼很簡單,只有2行。
function output = socpEndpoint(input)J = input.phase.integral;output.objective = J;
end
完整代碼
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 功能描述:剛體非對稱航天器控制問題
% 文件名解釋:mainSpacecraftOCP.m 中,main 代表 主函數,
% Spacecraft 代表 航天器,
% OCP 代表 最優控制問題
% 作者:Lei Lie
% 時間:2024/06/22
% 版本:1.0
% - 完成了代碼框架的初始搭建
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clc;clear;close all;
tic;
%% 01.初始參數設置
%-------------------------------------------------------------------------%
%----------------------- 設置問題的求解邊界 ------------------------------%
%-------------------------------------------------------------------------%
% 設置時間
t0 = 0;
tf = 100;
% 設置狀態量初值
w10 = .01;
w20 = .005;
w30 = .001;
% 設置狀態量邊界條件
w1_max = 1;
w1_min = 0;
w2_max = 1;
w2_min = 0;
w3_max = 1;
w3_min = 0;
% 設置控制量初值
u10 = -.009;
u20 = -.004;
u30 = -.001;
% 設置控制量邊界條件
u1_max = 0;
u1_min = -.01;
u2_max = 0;
u2_min = -.01;
u3_max = 0;
u3_min = -.01;
% 設置靜態參數
auxdata.I1 = 86.24;
auxdata.I2 = 85.07;
auxdata.I3 = 113.59;
% 設置不等式約束邊界條件(路徑約束)
path_max = .002;
path_min = 0;%% 02.邊界條件設置
%-------------------------------------------------------------------------%
%------------------------ 將求解邊界設置于問題中 -------------------------%
%-------------------------------------------------------------------------%
bounds.phase.initialtime.lower = t0;
bounds.phase.initialtime.upper = t0;
bounds.phase.finaltime.lower = tf;
bounds.phase.finaltime.upper = tf;
bounds.phase.initialstate.lower = [w10 w20 w30];
bounds.phase.initialstate.upper = [w10 w20 w30];
bounds.phase.state.lower = [w1_min w2_min w3_min];
bounds.phase.state.upper = [w1_max w2_max w3_max];
bounds.phase.finalstate.lower = [0 0 0];
bounds.phase.finalstate.upper = [0 0 0];
bounds.phase.control.lower = [u1_min u2_min u3_min];
bounds.phase.control.upper = [u1_max u2_max u3_max];
bounds.phase.integral.lower = 0;
bounds.phase.integral.upper = 10000;%% 03.初值猜測
%-------------------------------------------------------------------------%
%------------------------------- 初值猜想 --------------------------------%
%-------------------------------------------------------------------------%
guess.phase.time = [t0; tf];
guess.phase.state = [[w10 w20 w30];[0 0 0]];
guess.phase.control = [[u10 u20 u30];[u10 u20 u30]];
guess.phase.integral = 100;%% 04.設置GPOPS求解器參數
%-------------------------------------------------------------------------%
%---------------------------- 設置求解器參數 -----------------------------%
%-------------------------------------------------------------------------%
setup.name = 'Spacecraft-OCP';
setup.functions.continuous = @socpContinuous;
setup.functions.endpoint = @socpEndpoint;
setup.bounds = bounds;
setup.guess = guess;
setup.auxdata = auxdata;
setup.nlp.solver = 'ipopt';
setup.derivatives.supplier = 'sparseCD';
setup.derivatives.derivativelevel = 'second';
setup.mesh.method = 'hp1';
setup.mesh.tolerance = 1e-6;
setup.mesh.maxiteration = 45;
setup.mesh.colpointsmax = 4;
setup.mesh.colpointsmin = 10;
setup.mesh.phase.fraction = 0.1*ones(1,10);
setup.mesh.phase.colpoints = 4*ones(1,10);
setup.method = 'RPMintegration';%% 05.求解
%-------------------------------------------------------------------------%
%----------------------- 使用 GPOPS2 求解最優控制問題 --------------------%
%-------------------------------------------------------------------------%
output = gpops2(setup);
solution = output.result.solution;
toc;%% 06.畫圖
t = solution.phase.time(:,1);
w1 = solution.phase.state(:,1);
w2 = solution.phase.state(:,2);
w3 = solution.phase.state(:,3);
u1 = solution.phase.control(:,1);
u2 = solution.phase.control(:,2);
u3 = solution.phase.control(:,3);figure('Color',[1,1,1]);
plot(t,w1,'-','LineWidth',1.5);hold on;
plot(t,w2,'-.','LineWidth',1.5);
plot(t,w3,'--','LineWidth',1.5);
xlabel('Time',...'FontWeight','bold');
ylabel('States',...'FontWeight','bold');
legend('w1','w2','w3',...'LineWidth',1,...'EdgeColor',[1,1,1],...'Orientation','horizontal',...'Position',[0.5,0.93,0.40,0.055]);
set(gca,'FontName','Times New Roman',...'FontSize',15,...'LineWidth',1.3);
print -dpng spacecraft_ocp_state.pngfigure('Color',[1,1,1]);
plot(t,u1,'-','LineWidth',1.5);hold on;
plot(t,u2,'-.','LineWidth',1.5);
plot(t,u3,'--','LineWidth',1.5);
xlabel('Time',...'FontWeight','bold');
ylabel('Control',...'FontWeight','bold');
legend('u1','u2','u3',...'LineWidth',1,...'EdgeColor',[1,1,1],...'Orientation','horizontal',...'Position',[0.5,0.93,0.40,0.055]);
set(gca,'FontName','Times New Roman',...'FontSize',15,...'LineWidth',1.3);
print -dpng spacecraft_ocp_control.png%% 函數模塊部分
% ----------------------------------------------------------------------- %
% ------------------------- BEGIN: vsopcContinuous.m -------------------- %
% ----------------------------------------------------------------------- %
function phaseout = socpContinuous(input)w1 = input.phase.state(:,1);w2 = input.phase.state(:,2);w3 = input.phase.state(:,3);u1 = input.phase.control(:,1);u2 = input.phase.control(:,2);u3 = input.phase.control(:,3);I1 = input.auxdata.I1;I2 = input.auxdata.I2;I3 = input.auxdata.I3;dw1 = -((I3-I2)/I1) .* w2 .* w3 + u1 ./ I1;dw2 = -((I1-I3)/I2) .* w1 .* w3 + u2 ./ I2;dw3 = -((I2-I1)/I2) .* w1 .* w2 + u3 ./ I3;phaseout.dynamics = [dw1 dw2 dw3];phaseout.integrand = 0.5*(u1.^2 + u2.^2 + u3.^2);
end
% ----------------------------------------------------------------------- %
% -------------------------- END: vsopcContinuous.m --------------------- %
% ----------------------------------------------------------------------- %% ----------------------------------------------------------------------- %
% -------------------------- BEGIN: vsopcEndpoint.m --------------------- %
% ----------------------------------------------------------------------- %
function output = socpEndpoint(input)J = input.phase.integral;output.objective = J;
end
% ----------------------------------------------------------------------- %
% --------------------------- END: vsopcEndpoint.m ---------------------- %
% ----------------------------------------------------------------------- %
代碼仿真結果
狀態量:
控制量:
論文仿真結果:
狀態量:
控制量:
最后
歡迎通過郵箱聯系我:lordofdapanji@foxmail.com
來信請注明你的身份,否則恕不回信。