GAで二重振り子の入力トルク最小化の方法

Global Optimization Toolboxの遺伝的アルゴリズムgaを用いて,添付致しました二重振り子モデルの入力トルクの総和の最小化をしたいと思っています.

しかしモデル作成まではできたものの,どのようにコードを記述すれば良いのかわからず困っています.

例えば10秒後にそれぞれの軸の角度がpi/4[rad],速度が0になるような条件の下で入力トルクを総和を最小にするにはどのように記述すればよろしいでしょうか.指針やもしよろしければサンプルのソースコードを示していただけたら嬉しく思います.

ざっくりとした質問で大変恐縮ですが,ご教授頂けますと幸いです.

Answer 1

[Tohru Kikawada]

最適化を使って特定の問題を解く場合、最適化問題として定式化が必要になります。

その際考慮すべき項目は下記のとおりです。

1. 最適化変数
2. 目的関数
3. 制約

まず最適化変数をどのように置くかが課題になります。今回の問題の場合、10秒間で初期姿勢(0,0)から最終姿勢(pi/4,pi/4)になるような曲線を探索する問題と考えます。どのような曲線を定義するかによりますが、例えばスプライン曲線を考えます。その際、曲線の経由点を最適化変数として置いてみます。経由点の数は増やせば自由度が高まりますが、変数の数が増え、計算コストが上昇するため、トレードオフの関係になります。今回は経由点を4点としてみます。

つぎに目的関数をどうするかを考えます。今回はトルクの総和ということですので、与えられた角度に追従した際のトルクを逆動力学で計算し、積分して総和を求めます。

最後に制約を決めていきます。振り子と軸の干渉などを考慮する場合にはここに制約が必要ですが、今回は問題を簡単化するため、制約なしとします。

以上の定義ができれば後はコーディングしていくだけです。

最適化変数を与えたときに、目的関数がいくつになるか計算する部分はSimulink上でSimscape MulbitodyおよびSimulinkブロックを使って実装します。可変ステップで指定された精度で計算を行ってくれます。

一方、最適化計算(GA)の部分はMATLAB上でGlobal Optimization Toolboxを使って実装します。目的関数としてSimulinkのモデルを実行する関数を渡します。

トルク最小化したときの軌道を下記に示します。(例ではGAは時間がかかるので非線形最適化ソルバーを使用)

各軸の角度

各軸の角速度

プログラムを下記に示します。添付の trajectory.slx を同じフォルダに置いて実行してみてください。参考になれば幸いです。

function planTrajectory

%% Index
modelName = 'planning';
TORQUE_IND = 1;
Q_IND = 2;
DQ_IND = 3;

%% Open Simulink Model with Fast Restart Enabled
open_system(modelName);
set_param(modelName,'FastRestart','on');
set_param(modelName,'SimMechanicsOpenEditorOnUpdate','off');

%% Create initial trajectory using spline parametrization
t0 = 0;
q0 = [0 0];
qf = [pi/4 pi/4];
DQ0 = [0 0];
DQf = [0 0];
% Create linearly spaced time points and trajectories
totalTime = 10;
numPoints = 4;
numJoints = 2;
t = linspace(t0,totalTime,numPoints + 2);

% Qt = |q1(t0) q1(t1) ... q1(t_numPoints) q1(tf);
%      |q2(t0) q2(t1) ... q2(t_numPoints) q2(tf);
Q0 = zeros(numJoints, numPoints + 2);
for i = 1:numJoints
    Q0(i,:) = linspace(q0(i), qf(i),numPoints + 2);
end
Qt = Q0;

%% Set optimization problem
% Num Optimization variables = numJoints * numPoints
xOld = [];  % tracks previous optimal values
yout = [];  % simulation output

% Set cost function
fun = @(x) costFunTrajectoryPlanning(x);

% Set constraint function
nonlcon = [];

% Create initial optimization vector from spline matrix
% Select via points from Q matrix and expand into vector
% [Q0(:,2);Q0(:,3);Q(:,4),...];
x0 = Q0(:,2:numPoints+1);
x0 = x0(:);


A = [];
b = [];
Aeq = [];
beq = [];
lb = [];
ub = [];

% Optimization options
options = optimoptions('fmincon', 'Display', 'iter', 'Algorithm',...
    'interior-point', 'MaxIterations', 10, 'PlotFcn', @optimplotfval);

% Optimization options for GA
%options = optimoptions('ga', 'Display', 'iter', ...
%    'MaxGenerations',10,'PopulationSize',50,'PlotFcn', @gaplotbestf);

%% Solve optimization problem

[xOpt, fobj, exitFlag, outputFree] = fmincon(fun,x0,A,b,Aeq,beq,lb,ub,nonlcon,options);
%[xOpt, fobj, exitFlag,output]  = ga(fun,numJoints*numPoints,A,b,Aeq,beq,lb,ub,nonlcon,options);

% Convert optim vector to spline matrix
xOpt = reshape(xOpt, numJoints, numPoints);
QOpt = Q0;
QOpt(:,2:numPoints+1) = xOpt;

%% Get X and dX matrices from simulation
Qt = QOpt;
set_param(modelName,'FastRestart','off');
set_param(modelName,'SimMechanicsOpenEditorOnUpdate','on');
simOut = sim(modelName,'SrcWorkspace', 'current', 'StopTime', num2str(totalTime));
out = simOut.get('yout');
tout = simOut.get('tout');
q = out.getElement(Q_IND).Values.Data(:,:);
dq = out.getElement(DQ_IND).Values.Data(:,:);
tInd = ismembertol(tout,t);
tNew = tout(tInd);
if length(tNew) ~= length(t)
    error('planTrajectory: Time vectors don''t have the same length');
end
Qout = q(tInd,:)';
dQout = dq(tInd,:)';


%% Objective function: minimize Torque effort
    function [f] = costFunTrajectoryPlanning(x)
        updateIfNeeded(x);
        f = yout.getElement(TORQUE_IND).Values.Data(end,1);
        
    end

%% Helper function to run simulation only when needed
% See: http://www.mathworks.com/help/optim/ug/example-using-fminimax-with-a-simulink-model.html
    function updateIfNeeded(x)
        if ~isequal(x,xOld) % compute only if needed
            
            % Map optimization vector to spline matrix
            Xvp = reshape(x, 2, numPoints);
            Qt(:,2:numPoints+1) = Xvp;
            
            % Run simulation
            simOut = sim(modelName, 'SrcWorkspace', 'current',...
                'StopTime', num2str(totalTime));
            
            % Get Torque Cost function from matrix.
            % outputs: [torqueCost(1x1), dist2obj1(1x1)]
            yout = simOut.get('yout');
            
            xOld = x;
        end
    end

end