การควบคุมที่เหมาะสมที่สุดสำหรับลูกตุ้มง่ายๆ

ฉันกำลังศึกษาวิธีการควบคุมที่เหมาะสมที่สุด (และนำไปใช้ใน Matlab) และเป็นกรณีทดสอบที่ฉันเลือก (ตอนนี้) ลูกตุ้มง่าย ๆ (จับจ้องกับพื้นดิน) ซึ่งฉันต้องการควบคุมให้อยู่ในตำแหน่งบน

ฉันจัดการเพื่อควบคุมโดยใช้วิธีป้อนกลับแบบ "ง่าย" (แกว่งขึ้นบนพื้นฐานของการควบคุมพลังงาน + เสถียรภาพ LQR สำหรับตำแหน่งบน) และวิถีการแสดงสถานะเป็นภาพ (ฉันลืมคำอธิบายแกน: x คือทีต้า, y คือทีต้า จุด

ตอนนี้ฉันต้องการลองวิธีการควบคุมที่ดีที่สุด "เต็ม" เริ่มต้นด้วยวิธี 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 ในตำแหน่งที่ไม่เสถียร / เป้าหมาย) 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 จัดหาสคริปต์ทดสอบที่ควรอธิบายแนวทางแม้ว่าอาจจำเป็นต้องปรับแต่งสำหรับระบบของคุณ