GPS-автопилот - алгоритъм

[juliang]

Ще те оставя да го пробваш в реални условия за да се убедиш че не е най-доброто решение.
В почти всички случаи е много по-добре интегралната част да ползва записани данни от много по-стари събития от деривативната. В твоя случай деривативната ще измери скоростта на изменение за много голям период назад, и повече ще пречи отколкото да помага. Ей-таака на усет в твоя случай бих я взел спрямо 4-я или 5-я елемент, не от 9-я.
Предопоследния ред същи нещо не го схващам какво прави...
И още нещо - гледам ползваш float. Замисли се дали да не реализираш изискващия се от добрата практика anti-windup контрол на интегралната поправка (какъвто аз нямам в моя код ) Примерно взимаш само дробната част на грешката - частта след дес. запетайка. Със знака, естествено. Ако грешката е голяма - пропорционалната сама ще се справи, няма смисъл и интегралната да напъва заедно с нея. А когато грешката е малка, интегралната вече ще може да  коригира това, което пропорционалната не е успяла.
Но всичко зависи от това в какъв диапазон ще са ти входните и изходните данни.

[dmitarp]

Аз бих го направил така:

int PID_regulator(float input, float setpoint, float kp, float ki, float kd, int minOut, int maxOut) {
 
  static float error;                                          // текуща грешка
  float derivative = 0;                                             //дефиринциална съставна
  static int Output = 0;                                           //изход - връща значение

  error = setpoint - input;                                     //текущо значение на грешката
 
  integral  +=  Ki*error;                                                         //интегрална съставна
  derivative = (error - error_old) * Kd;                  //деферинциална съставна
  Output = error* kp + integral + derivative;       //събираме трите съставни в изхода
  if(Output < minOut) Output = minOut;                    //ограничаване по минимум
  else if(Output > maxOut) Output  = maxOut;          //ограничаване по максимум
  else Output;                                                              //четем изхода без ограничения
  error_old = error                                                      // запазване на старата стойност на грешката
  return Output;                                                          //връщаме значението от изхода
 }


Променливите integral и error_old са глобални. Времето за интегриране и диференциране е замаскирано в стойностите на Ki и Kd съответно.
Процеса на регулиране е непрекъснат затова не е добра идея тази функция да се извиква през 5 секунди. Дори системата да е много инертна за тези 5 секунди остава валидно Output без промяна.

Настройката на регулатора се прави по следният начин:
Ki = 0
Kd = 0
Задаваш само стойност на Kp. От тази стойност зависи времето на реакция, и в известна степен крайната грешка след установяване. Колкото е по-голямо Kp и времето за реакция ще се намали, разбира се това време няма как да е по-малко от възможностите на изпълнителният механизъм. След като си удволетворен от времето на реакция, задаваш стойност на Kd, от него зависи пулсациите преди крайното установяване, както и времето за крайното установяване, колкото е по голямо Kd пулсациите намаляват и времето за крайното установяване намалява. И накрая като дадеш стойност на Ki, ще можеш да нагласиш грешката след установяването, така че да бъде близка до нула. Трябва да се избягват големи стойности на Ki защото се увеличава времето за установяване и пулсациите.

[EDM electronics]

1. Предопоследния ред същи нещо не го схващам какво прави...

2. И още нещо - гледам ползваш float. Замисли се дали да не реализираш изискващия се от добрата практика anti-windup контрол на интегралната поправка (какъвто аз нямам в моя код )

1. Този ред е ненужен, сложил съм го по инерция. Всъщност това е реализация на една функция от С++, това е constrain(x, min, max) за оразмеряване на променлива и този трети ред има смисъл, ако функцията се ползва самостоятелно. Тук я ползвам вътре в друга функция и този ред се явява излишен, защото всъщност го чете от сбора по-горе.

2. Ползвам float защото входните ми данни са метри /разстояние от една GPS-точка до друга/ и там числото е веществено, за да отчита и дециметри. Нужно е, защото диапазона на входа който ме интересува е 3-4 м. Това е максимално допустимото разстояние на което ще ме мести течението, да го наречем дрейф, а и то е съобразено с точността на GPS-а, който за граждански цели е 3-5 м. но това е точност, която отговаря на самата география, иначе се надявам точността между 2 точки да е поне 1 м., без оглед на географията. Та входа ми е от 0-4 м число float.

Изхода ще е стойността на числото, което регулира оборотите на двигателя, да ме върне в запаметената точка с такава скорост, каквото е течението в момента, т.е. да отговаря на обстановката. От опит знам какъв трябва де е диапазона и скоростта - от 0 до 3 км/ч или това е число от 0-40. Изхода вече е не веществено число, не е необходим float. Имам вход 0-4 и изход от 0-40.

Системата както казах е доста инертна. Току така изневиделица течение не се появява, има само порив на вятъра, който също ще ме бута. Та един такъв порив не трябва да ми сменя скоростта, само тенденция.

Понеже съм наясно с входно-изходните параметри, ще мога да направя настройката на ПИД-а директно с плотера на компилатора. Само трябва да симулирам с един таймер различни стойности на входа и на графика ще виждам идеално какво става с изходните данни. Това ще го направя съвсем скоро.

[EDM electronics]

Аз бих го направил така:

int PID_regulator(float input, float setpoint, float kp, float ki, float kd, int minOut, int maxOut) {
 
  static float error;                                          // текуща грешка
  float derivative = 0;                                             //дефиринциална съставна
  static int Output = 0;                                           //изход - връща значение

  error = setpoint - input;                                     //текущо значение на грешката
 
  integral  +=  Ki*error;                                                         //интегрална съставна
  derivative = (error - error_old) * Kd;                  //деферинциална съставна
  Output = error* kp + integral + derivative;       //събираме трите съставни в изхода
  if(Output < minOut) Output = minOut;                    //ограничаване по минимум
  else if(Output > maxOut) Output  = maxOut;          //ограничаване по максимум
  else Output;                                                              //четем изхода без ограничения
  error_old = error                                                      // запазване на старата стойност на грешката
  return Output;                                                          //връщаме значението от изхода
 }


Променливите integral и error_old са глобални. Времето за интегриране и диференциране е замаскирано в стойностите на Ki и Kd съответно.
Процеса на регулиране е непрекъснат затова не е добра идея тази функция да се извиква през 5 секунди. Дори системата да е много инертна за тези 5 секунди остава валидно Output без промяна.

Настройката на регулатора се прави по следният начин:
Ki = 0
Kd = 0
Задаваш само стойност на Kp. От тази стойност зависи времето на реакция, и в известна степен крайната грешка след установяване. Колкото е по-голямо Kp и времето за реакция ще се намали, разбира се това време няма как да е по-малко от възможностите на изпълнителният механизъм. След като си удволетворен от времето на реакция, задаваш стойност на Kd, от него зависи пулсациите преди крайното установяване, както и времето за крайното установяване, колкото е по голямо Kd пулсациите намаляват и времето за крайното установяване намалява. И накрая като дадеш стойност на Ki, ще можеш да нагласиш грешката след установяването, така че да бъде близка до нула. Трябва да се избягват големи стойности на Ki защото се увеличава времето за установяване и пулсациите.

dmitarp това което представяш е всъщност кода, който се подвизава навсякъде в Интернет за ПИД-регулатор. Т.е. имаме за сравнение само две стойност - текуща и стара, предишна. Аз възприех идеята на Юлиан, защото той ме убеди с тази оценка на бъдещето по повече на брой измервания в миналото.

Обаче така както си го представил променливата integral нищо не я нулира при всяка итерация тя ще натрупва стойност, като брояч. После мисля, че няма нужда да е глобална, умишлено не съм писал глобални, за да се ползва функцията универсално, без да се добят извън нея променливи и затова си има лесно - правят се статични вътре в самата функция. Така "живота" им се запазва.

В случая обаче променливата integral не е нужно да запазва стойността си, може да се нулира предварително автоматично, когато не е статична или дори да е статична ще приема следващата стойност на сбора.

[dmitarp]

В случая интегралната съставна използва всички минали стойности, и няма как да се натрупа безкрайно стойност, защото грешката може да бъде и положителна и отрицателна. За дефиренциалната съставка може да се ползва и по стари данни, всяка от коита да идва с нейната си тежест, но това не променя почти нищо, всичко зависи от времето за актулизация, т.е. от тези 5 секунди, ако станат примерно 0,05 секунди тогава не ти трябват повече от една стара стойност, но ако си останат 5 секунди, колкото и стари стойности да вземеш, по-голяма точност няма да постигнеш.

[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]

2. Ползвам float защото входните ми данни са метри /разстояние от една GPS-точка до друга/ и там числото е веществено, за да отчита и дециметри. Нужно е, защото диапазона на входа който ме интересува е 3-4 м. Това е максимално допустимото разстояние на което ще ме мести течението, да го наречем дрейф, а и то е съобразено с точността на GPS-а, който за граждански цели е 3-5 м. но това е точност, която отговаря на самата география, иначе се надявам точността между 2 точки да е поне 1 м., без оглед на географията. Та входа ми е от 0-4 м число float.
Изхода ще е стойността на числото, което регулира оборотите на двигателя, да ме върне в запаметената точка с такава скорост, каквото е течението в момента, т.е. да отговаря на обстановката. От опит знам какъв трябва де е диапазона и скоростта - от 0 до 3 км/ч или това е число от 0-40. Изхода вече е не веществено число, не е необходим float. Имам вход 0-4 и изход от 0-40.

Задачата ти няма да е толкоз проста, защото ти ще трябва да регулираш два параметъра - скорост на мотора и завой на руля. Трябват ти два PID регулатора - единия ще работи с мотора по разстоянието, другия - с руля по ъгъл на отклонение от зададения курс. А курса ще се променя с положението на лодката... цирка ще е голям

Обаче така както си го представил променливата integral нищо не я нулира при всяка итерация тя ще натрупва стойност, като брояч.

Така и трябва да бъде на теория - смисъла на интегралната поправка е да натрупва малката грешка и тя с времето да накара регулатора да направи стъпка, дори и ако работиш в метри, а грешката е милиметър. На практика много рядко се налага да се прави корекция при толкова малка грешка. Затова и ти споменах за anti-windup ограничението на интегралната корекция.
Но пък да се ползва само последната една или две стойности е неправилно - така тая корекция губи смисъла си. И дифренциалната и тя до голяма степен се обезсмисля.

П.С. ПИД за разстоянието е малко овъркил - можеш просто да кажеш "ако разстоянието до целта ми е над 100 метра - мотора работи на 100%, ако е под 100 метра - скоростта му в проценти е равна на разстоянието в метри до целта

[EDM electronics]

Задачата ти няма да е толкоз проста, защото ти ще трябва да регулираш два параметъра - скорост на мотора и завой на руля. Трябват ти два 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 и закръгляне