Справочници, схемотехника, теория > Програмиране на микроконтролери, програматори, цифрови проекти
GPS-автопилот - алгоритъм
EDM electronics:
Да, интегралната съставна навсякъде я дават с натрупване и вероятно от колебанията положителна/отрицателна запазва разумна стойност. Въпреки това съм я поставил в ограничения, ако излезе извън обхват.
Ето го и варианта с отчет само на текущо и предходно измерване /само 2 измервания/ на входния параметър:
int PID_regulator(float input, float setpoint, float kp, float ki, float kd, float dt, int minOut, int maxOut) {
static float prev_error = 0; //предходна грешка
static float integral = 0; //интегрална съставна
static int Output = 0; //изход - връща значение
float error = setpoint - input; //грешка - пропорционална съставна
integral = integral + error * dt * ki; //интегрална съставна
if(integral < minOut) integral = minOut; //ограничаване по минимум
else if(integral > maxOut) integral = maxOut; //ограничаване по максимум
float derivative = (error - prev_error) / dt; //диференциална съставна
prev_error = error ;
Output = error * kp + integral + derivative * kd; //събираме
if(Output < minOut) Output = minOut; //ограничаване по минимум
else if(Output > maxOut) Output = maxOut; //ограничаване по максимум
return Output; //връщаме значението от изхода
}
juliang:
--- Цитат на: EDM electronics в Април 28, 2022, 09:06:41 am ---2. Ползвам float защото входните ми данни са метри /разстояние от една GPS-точка до друга/ и там числото е веществено, за да отчита и дециметри. Нужно е, защото диапазона на входа който ме интересува е 3-4 м. Това е максимално допустимото разстояние на което ще ме мести течението, да го наречем дрейф, а и то е съобразено с точността на GPS-а, който за граждански цели е 3-5 м. но това е точност, която отговаря на самата география, иначе се надявам точността между 2 точки да е поне 1 м., без оглед на географията. Та входа ми е от 0-4 м число float.
Изхода ще е стойността на числото, което регулира оборотите на двигателя, да ме върне в запаметената точка с такава скорост, каквото е течението в момента, т.е. да отговаря на обстановката. От опит знам какъв трябва де е диапазона и скоростта - от 0 до 3 км/ч или това е число от 0-40. Изхода вече е не веществено число, не е необходим float. Имам вход 0-4 и изход от 0-40.
--- Край на цитат ---
Задачата ти няма да е толкоз проста, защото ти ще трябва да регулираш два параметъра - скорост на мотора и завой на руля. Трябват ти два PID регулатора - единия ще работи с мотора по разстоянието, другия - с руля по ъгъл на отклонение от зададения курс. А курса ще се променя с положението на лодката... цирка ще е голям :)
--- Цитат на: EDM electronics в Април 28, 2022, 09:17:34 am ---Обаче така както си го представил променливата integral нищо не я нулира при всяка итерация тя ще натрупва стойност, като брояч.
--- Край на цитат ---
Така и трябва да бъде на теория - смисъла на интегралната поправка е да натрупва малката грешка и тя с времето да накара регулатора да направи стъпка, дори и ако работиш в метри, а грешката е милиметър. На практика много рядко се налага да се прави корекция при толкова малка грешка. Затова и ти споменах за anti-windup ограничението на интегралната корекция.
Но пък да се ползва само последната една или две стойности е неправилно - така тая корекция губи смисъла си. И дифренциалната и тя до голяма степен се обезсмисля.
П.С. ПИД за разстоянието е малко овъркил - можеш просто да кажеш "ако разстоянието до целта ми е над 100 метра - мотора работи на 100%, ако е под 100 метра - скоростта му в проценти е равна на разстоянието в метри до целта :)
EDM electronics:
--- Цитат на: juliang в Април 29, 2022, 08:45:12 pm ---Задачата ти няма да е толкоз проста, защото ти ще трябва да регулираш два параметъра - скорост на мотора и завой на руля. Трябват ти два PID регулатора - единия ще работи с мотора по разстоянието, другия - с руля по ъгъл на отклонение от зададения курс. А курса ще се променя с положението на лодката... цирка ще е голям :)
--- Край на цитат ---
По принцип алгоритъма за котвата не е лесен, но не и невъзможен и съвсем скоро ще го напиша. Почти съм го измислил. Но да, за да няма полудяване на сервото и основния мотор, трябват именно 2 ПИД-регулатора, за скоростта на мотора и руля. Ако на руля няма ПИД, а той се следи от компас, който е доста чувствителен, ако се управлява пропорционално, както съм го направил сега само временно, ще полудее и ще движи в зиг-заг, а на практика като движа руля ръчно няма зиг-заг, всичко си е плавно, това ще прави и ПИД-а.
Основният въпрос на ПИД-а за скоростта на мотора при режим котва беше, кой параметър да го играе вход и кой сетпойнт. В случая ще е наобратно. Входа ще е стойността в която искам винаги да ме кара двигателя или това е 0-метри от запомнената точка, а вече изместването или разстоянието на което ме е изместило течението ще го играе сетпойнта. Когато съм най-отдалечен от точката и течението е най-силно, трябва стойността на мотора да е най-голяма и обратно. Трябва тоя ПИД да се стреми постоянно да ме кара на 0-метра, но всъщност ще го поставя на 1-2 м, защото GPS няма да може да ми даде баш 0-метра, има дрейф на показанията му и точността е около 2 м. В противен случай винаги ще пропуска тези 0 метра и ще ме праща в обратно направление и така става същото, като зиг-зага на руля. Та точност от 2-3 м за котва е в кръга на нормалното, то няма как и да стане по точно заради GPS-а.
Тия дни ще изпробвам ПИД за мотора.
juliang:
В някои системи а управленеи се ползва и т.нар. "dead zone", или отклонение което е допустимо или невъзможно за неутрализиране. Нещо от рода на "ако се намирам в радиус от 5 метра от зададената точка, не предприемай нищо - достатъчно точно е". Това се прави или чрез подбиране на достатъчно "груби" входни даннни (твоя float е точно обратното на това :) ) или чрез математически операции.
Ако ползваш int за вход, и една единица ти е 1 метър, тогава грешка под 1 метър просто няма да може да се отчете като отклонение. Можеш да мащабираш входа така че единица да ти е 2 или 5 метра, и тогава такова отклонение ще се приема като "точно в целта".
Затова ти казвах, че секундите са безмислени в такъв процес. Не се отнася само за секундите, и метрите също са в тази категория. Понякога прекаленото спазване на "каноните" почва да пречи.
Ако знаеш че отклонението ти по дистанция е 2 единици, а отклонението ти по ъгъл е 5 единици вляво няма никакво значение тези единици какво означават - метри или сантиметри, градуси или радиани... Силата на реакция на контролера ще може да се компенсира с подбиране на подходящи коефициенти Kp, Ki и Kd. Просто единиците с които ще работиш трябва да са достатъчно малки, за да ти осигурят търсената точност, но и достатъчно груби за да не карат пид-а да реагира на 5 мм или една ъглова минута отклонение, която точност за практическата задача която ще изпълняваш е ненужна.
EDM electronics:
Именно това съм предвидил и аз, като поставям не 0, а 1-2 м за тая мъртва зона. Но защо ползвам флоат:
Понякога GPS-а е свръхточен, до дециметри. Явно зависи от времето, температурата, проходимостта на сигнала - влажност и т.н., а също и от мястото. Ако на едно място имам голяма точност, на друго тя ще е 2-3 м.
Ползвам флоат, защото така или иначе имам такава променлива от формулата за изчисление на дистанцията между две точки. Но за автопилота не ми трябва такова число със запетая, защото останалата част от кода са цели числа, го преобразувам и закръглям.
GPS_distance = (uint16_t) round (distance);
На етап проба е все едно какво число ще ползвам, така и така го имам. distance е флоат, а GPS_distance е цяло закръглено число. По същия начин съм направил с компаса. Той има точност стотни от градуса, но съм го преобразувал до 1 градус в цяло число.
case 3: abc = bno055_getVectorEuler(); //функция компас
compas = (uint16_t) round(abc.x); //премахване на float и закръгляне
Навигация
[0] Списък на темите
Премини на пълна версия