简单摆的最优控制

我正在研究各种最佳控制方法（并在Matlab中实现它们），并且作为测试用例，我现在选择一个简单的摆锤（固定在地面上）作为测试用例，将其控制在较高位置。

我设法使用“简单”的反馈方法（基于能量控制的摆动+上部位置的LQR稳定）对其进行控制，并且状态轨迹如图所示（我忘记了轴的描述：x是theta，y是theta点。

现在，我想尝试一种“完整”的最佳控制方法，从迭代LQR方法开始（我发现在这里实现了该方法http://homes.cs.washington.edu/~todorov/software/ilqg_det.m）

该方法需要一个动态函数和一个成本函数（x = [theta; theta_dot], u为电动机转矩（仅一台电动机））：

function [xdot, xdot_x, xdot_u] = ilqr_fnDyn(x, u)
    xdot = [x(2);
        -g/l * sin(x(1)) - d/(m*l^2)* x(2) + 1/(m*l^2) * u];
    if nargout > 1
        xdot_x = [ 0, 1;
            -g/l*cos(x(1)), -d/(m*l^2)];
        xdot_u = [0; 1/(m*l^2)];
    end
end

function [l, l_x, l_xx, l_u, l_uu, l_ux] = ilqr_fnCost(x, u, t)
    %trying J = x_f' Qf x_f + int(dt*[ u^2 ])
    Qf = 10000000 * eye(2);
    R = 1;
    wt = 1;
    x_diff = [wrapToPi(x(1) - reference(1)); x(2)-reference(2)];

    if isnan(t)
        l = x_diff'* Qf * x_diff;
    else
        l = u'*R*u;
    end

    if nargout > 1
        l_x = zeros(2,1);
        l_xx = zeros(2,2);
        l_u = 2*R*u;
        l_uu = 2 * R;
        l_ux = zeros(1,2);

        if isnan(t)
            l_x = Qf * x_diff;
            l_xx = Qf;
        end
    end
end

有关摆锤的一些信息：系统的原点是摆锤固定在地面上的位置。角度θ在稳定位置为零（pi在不稳定/目标位置为pi）。 m是摆锤质量，l在杆长度，d是阻尼因子（为简单起见，我把m=1，l=1，d=0.3）

我的成本很简单：惩罚控件+最终错误。

这就是我调用ilqr函数的方式

tspan = [0 10];
dt = 0.01;
steps = floor(tspan(2)/dt);
x0 = [pi/4; 0];
umin = -3; umax = 3;
[x_, u_, L, J_opt ] = ilqg_det(@ilqr_fnDyn, @ilqr_fnCost, dt, steps, x0, 0, umin, umax);

这是输出

时间从0到10。初始条件：（0.785398,0.000000）。目标：（-3.141593,0.000000）长度：1.000000，质量：1.000000，阻尼：0.300000

使用迭代LQR控制

迭代次数= 5；费用= 88230673.8003

标称轨迹（即控件找到的最佳轨迹）为

控件“关闭” ...它甚至没有尝试达到目标...我在做什么错？（Todorov的算法似乎可以正常工作。

无需遍历所有代码（这太像真实的工作了），我的直觉是您已对控制工作进行了足够的加权，以至于成本最低的事情是不执行任何操作并忍受错误。

是的，我知道-您所有明确的权重都是统一的。但是，仍然-尝试将控制权重降低，或者将位置误差权重提高。

同样，在不深入了解代码的情况下，您的ilrq函数可能无法“理解”您所控制的事物的非线性性质。因此，它可能找不到通往摆直立位置的方法，并且可能再次失败。

您首先尝试的方法是将适当量的能量放入钟摆中，然后在钟摆直立后进行最佳调节，这可能是最好的方法：您知道在没有摩擦的情况下，系统只有正确的摆放位置大量的能量最终将停留在最上面（但是只是短暂地），所以这似乎是一个明智的起点。

iLQR是一种迭代方法，但实际上您似乎并没有在迭代。Todorov提供了一个测试脚本，该脚本应阐明该方法，尽管可能需要为您的系统定制该方法。