forum.boolean.name

Решил сделать робота - балансировщика. Так как пока железа нет, то скачал себе
v-rep: http://www.coppeliarobotics.com/

Запилил робота, все такое, но никак не могу отрегулировать ПИД-регулятор.
Значения получаю с гироскопа и акселерометра (виртуальных).

В качестве фильтра юзаю простой, компланарный:
http://www.poprobot.ru/theory/complementary_filter

с K = 0.95

ПИД - регулятор рекурентный, интегрирование трапецией, но он либо в автоколебания уходит, либо линейно растет:mad:.

Может я его не верно написал?
ПИД:

Код:

--pid

Kp, Ki, Kd = 0.25,1,.2



--Kp, Ki, Kd = .085, 0.00, 0.0 



------------------------- pid



U, oldErr, oldErr2, I, D, oldTime = 0,0,0,0,0,0



PID_regulator = function() 



    _time = simGetSimulationTime()

    dt = _time - oldTime

    

    local err = getOrient(dt)[2]

    

    P = (err-oldErr) * Kp

    I = Ki*Kp*dt*(oldErr + err)*0.5 



    if(dt ~= 0) then

        D = (err - 2*oldErr + oldErr2) * Kd*Kp / (dt)

    else

        D = 0

    end



    U = U + P + I + D

    

    oldErr2 = oldErr

    oldErr = err

    oldTime = _time

    if(graph ~= nil) then

        simSetGraphUserData(graph, "PID", U*1000)

    end

    return U

end

Вот после нескольких часов подбирания коэффициентов, но отзывчивость этого PID никакая, так как интеграл огромный:

Левый график - график PID и колес, правый - значение с сенсоров ориентации.

Видео на YouTube

Кстати, кто хочет поиграться, то см. вложение.
Вложение 22394