拉格朗日方程的数值解（Matlab）

贡献者： addis

　　 若程序中给出拉格朗日量拉格朗日方程的数值函数 $L(q, \dot q, t)$，输入和输出均为数值（例如双精度实数），那么如何数值求解运动方程呢？

　　 问题的关键是如何列出一个一阶常微分方程组。如果可以给出系统哈密顿量的数值函数，那么数值解哈密顿正则方程是首选的做法，因为它本身已经是一阶常微分方程组，可以直接用解算器进行数值解。但对于较复杂的系统，哈密顿量比拉格朗日量难算得多，甚至可能没有解析表达式。

　　 相比之下，拉格朗日量虽然容易写出，但方程的数值解比哈密顿正则方程要难一些，且如果用差分法计算微分会引入一定的数值误差。

　　 由拉格朗日方程（式 3 ）

　　 代码依赖 “偏导与差分” 中的 D2_ij.m，以及 “导数与差分” 的 D_i.m。

% 数值解拉格朗日方程
% 拉格朗日量 L(qqt), qqt = [q, dq, t]
% 一阶偏微分 dL(i,qqt), 可选, 若不支持 i > N， 返回 nan
% 二阶偏微分 d2L(i,j,qqt) 可选, i = N+1:2*N, j = 1:2*N+1
% 广义力 Q(i, qqt) 可选
function qq = lagr_solve(L, qq0, t, RelTol, dL, d2L, Q, vpa_flag)
%====== 参数设置 =======
h = 1e-5; % 1 阶差分长度
h2 = 1e-5; % 2 阶差分长度
%=====================
N = numel(qq0)/2;
if ~exist('vpa_flag', 'var') || isempty(vpa_flag)
    vpa_flag = false;
end
if ~exist('d2L', 'var') || isempty(d2L)
    if ~exist('dL', 'var') || isempty(dL) || isnan(dL(N+1,[qq0, tmin]))
        if vpa_flag
            d2L = @(i,j,qqt)D2_ij_vpa(L,i,j,qqt,h2);
        else
            d2L = @(i,j,qqt)D2_ij(L,i,j,qqt,h2);
        end
    else
        d2L = @(i,j,qqt)d2L_dif1(dL,i,j,qqt,h);
    end
end
if ~exist('dL', 'var') || isempty(dL)
    if vpa_flag
        dL = @(i, qqt)D_i_vpa(L, i, qqt, h);
    else
        dL = @(i, qqt)D_i(L, i, qqt, h);
    end
end
if ~exist('Q', 'var') || isempty(Q)
    Q = @(i,qqt) 0;
end
options = odeset('RelTol', RelTol);
[T, QQ] = ode45(@(t,qq)ode_fun(t, qq, dL, d2L, Q), ...
        [t(1), t(end)], qq0, options);
qq = zeros(numel(t), 2*N);
for i = 1:2*N
    qq(:,i) = interp1(T, QQ(:,i), t, 'spline');
end
end

function dqq = ode_fun(t, qq, dL, d2L, Q)
N = numel(qq)/2;
dqq = zeros(2*N, 1);
dqq(1:N) = qq(N+1:end);
qqt = [qq; t];
A = zeros(N, N); % A_ij = d2L/d(\dot q_i)d(\dot q_j)
for i = 1:N
    for j = i:N
        A(i,j) = d2L(i+N, j+N, qqt);
        if i ~= j
            A(j,i) = A(i,j);
        end
    end
end
b = zeros(N, 1);
for i = 1:N
    b(i) = dL(i, qqt) - d2L(i+N,2*N+1,qqt) + Q(i,qqt);
    for j = 1:N
        b(i) = b(i) - d2L(N+i,j,qqt)*qqt(j+N);
    end
end
dqq(N+1:end) = A \ b;
if any(isnan(dqq) | isinf(dqq))
    error('wrong!');
end
end

% d^2 L / d(qqt_i)d(qqt_j)
% dL is dL/d(qqt_i)
function ret = d2L_dif1(dL, i, j, qqt, h)
qqt(j) = qqt(j) - 0.5*h;
L1 = dL(i, qqt, h);
h = (qqt(j) + h) - qqt(j);
qqt(j) = qqt(j) + h;
L2 = dL(i, qqt, h);
ret = (L2 - L1) / h;
end

　　 用双摆的例子来测试，比较 “双摆的数值计算（Matlab）” 的结果：

% lagr_solve_demo
function lagr_solve_demo
% ==== 参数 =====
m1 = 1; m2 = 1; g = 9.8;
l1 = 1; l2 = 1;
tmin = 0; tmax = 10; Nt = 200;
qq0 = [pi, pi, -4, 6];
RelTol = 1e-6;
L = @(qqt)L_fun(qqt, m1, l1, m2, l2, g);
% ==============
close all;
t = linspace(tmin, tmax, Nt);
qq = lagr_solve(L, qq0, t, RelTol);

th1 = qq(:,1); th2 = qq(:,2);
x1 = l1*sin(th1); y1 = -l1*cos(th1);
x2 = x1 + l2*sin(th2); y2 = y1 - l2*cos(th2);
figure;
% 动画
for it = 1:Nt
    clf;
    plot([0, x1(it)], [0, y1(it)], 'k'); hold on;
    scatter(x1(it), y1(it), 'k');
    plot([x1(it), x2(it)], [y1(it), y2(it)], 'k');
    scatter(x2(it), y2(it), 'k');
    plot(x2(1:it), y2(1:it));
    axis equal; axis([-1,1,-1,1]*(l1+l2)*1.1);
    title(['t = ' num2str(t(it), '%.2f')]);
    xlabel x; ylabel y;
    pause(0.1); % saveas(gcf, [num2str(it) '.png']);
end
end

% 双摆
function ret = L_fun(qqt, m1, l1, m2, l2, g)
th1 = qqt(1); th2 = qqt(2);
w1 = qqt(3); w2 = qqt(4);
T = 0.5*m1*(l1*w1)^2 + 0.5*m2*((l1*w1*cos(th1) + l2*w2*cos(th2))^2 ...
    + (l1*w1*sin(th1) + l2*w2*sin(th2))^2);
V = -m1*g*l1*cos(th1) - m2*g*(l1*cos(th1) + l2*cos(th2));
ret = T - V;
end

1. ^ 如果 $N$ 不大可以直接用克拉默法则，甚至写出解析解。