Решил сделать робота - балансировщика. Так как пока железа нет, то скачал себе
v-rep:
http://www.coppeliarobotics.com/
Запилил робота, все такое, но никак не могу отрегулировать ПИД-регулятор.
Значения получаю с гироскопа и акселерометра (виртуальных).
В качестве фильтра юзаю простой, компланарный:
http://www.poprobot.ru/theory/complementary_filter
с K = 0.95
ПИД - регулятор рекурентный, интегрирование трапецией, но он либо в автоколебания уходит, либо линейно растет
.
Может я его не верно написал?
ПИД:
--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 и колес, правый - значение с сенсоров ориентации.
Кстати, кто хочет поиграться, то см. вложение.
STABILYTY.rar