简单摆的最优控制

我正在研究各种最佳控制方法（并在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，角度θ为零）在不稳定/目标p中
m是摆锤质量，l是杆长，d是阻尼因子（为简单起见，我将m=1，l=1和d=0.3）

我的成本很简单：对控制+最终错误。

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

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

使用迭代LQR控制

迭代次数= 5；成本= 88230673.8003


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


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