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

[VITAN]

Браво, браво 
Е, с 2 ардуино-та ще стане 

[EDM electronics]

Сблъсках се със следния проблем:
Счетох, че вътрешната памет на Ардуино НАНО е твърде малка за запис на маршрути и поставих външна 24CL128. Това е огромна памет за целта.
Проблема обаче се състои в това, че езикът IDE /компилатора най-вече/ възприема най-голямо число с променлива запетая тип float. То е с големина 4 байта. Има и double, но то е същото float.
При смятане с такива числа или запис в паметта точността се ограничава до 7 разряда, независимо от запетаята. Т.е. точността на 4235672.3 е същата като 42.356723. Последната цифра /осмия разряд/ варира с 2-3 стойности.

Някой от програмистите, имате ли идея как да го направя с абсолютна точност до 8 разряд?

Пробвах при запис с умножение по сто, а при четене с деление на 100, но не става, запазва се точността до 7 разряд, осмия вече варира.

Записа правя, като записвам 4 отделни байта с разбиване на числото float на 4, а при четене се четат отделните байтове и се сумира в тип float . Ползвам две отделни функции за запис и четене.

#include <Eeprom24C128_256.h>
#define EEPROM_ADDRESS  0x50
static Eeprom24C128_256 eeprom(EEPROM_ADDRESS);

void setup()
{
  float  val_1 = 42.899751;
  float  val_2 = 0;
  Serial.begin(9600);
  eeprom.initialize();
 
  EEPROM_float_write(0, val_1);
  for (int ii = 0; ii<4; ii=ii+4)
  {
    val_2 = EEPROM_float_read(ii);

  // запис в серниния порт
 
  Serial.println(val_2,6);
  }
 
}

void loop(){}


void EEPROM_float_write (int addr, float  val) // запис в ЕЕПРОМ
{ 
  byte *x = (byte *)&val;
  for(byte i = 0; i < 4; i++)
  {
    eeprom.writeByte(i+addr, x);
    delay(10);
  }
}

float  EEPROM_float_read (int addr) // четене от ЕЕПРОМ
{   
  byte x[4];
  for(byte i = 0; i < 4; i++) x = eeprom.readByte(i+addr);
  float  *y = (float *)&x;
  delay(10);
  return y[0];
}

[juliang]

Ако работиш с числа с плаваща запетая нещата вероятно ще са малко по-добри, но трябва да внимаваш изключително много при преминаването от абсолютни географски координати към разстояния. Не е изключено катастрофално да загубваш точност, защото ардуиното си работи с числа със 7-8 значещи цифри.

Аз като говоря не приказвам...
Решението е да вземеш една сравнително близка точка за "нула" и да работиш с цели числа спрямо нея. 4 байта са +/- 2 милиарда, което ако работиш в сантиметри са +/- 20 000 километра. Така че ако смяташ в цели числа дори и ако приемеш нулата за нула градуса северна и нула градуса източна... пак България ще влезе в обхвата. А ако хванеш една точка в България, или примерно 45 градуса северна и 45 градуса източна, то ще можеш да обхванеш цяла България дори и в милиметри (което е абсолютно безмислено).

Обаче ти искаш да ползваш синуси, тангенси, квадратни корени ... дето връщат нецели числа... Не че не става, но пак ще трябва да мислиш какво се случва и кога е възможно рязко да загубиш точност.

НЕ МОЖЕШ да работиш с 8-я разряд, защото сметките ти ще са грешни. Дробните числа се представят като а*1/2 + b*1/4 + c*1/8 + d*1/16 + .... (а, b, c, d.... са битове и са 0 или 1) и просто няма как да запишеш точно 1/3 или 1/7. Затова вътрешно се смята с 8 знака точност, но ти се дава само до 7-я, който със сигурност е точен, а 8-я се спестява защото не винаги отговаря на истината.

[EDM electronics]

Ползвам формулата с тангенси, котангенси и т.н. предимно защото само с нея мога да реализирам котвата. Не че твоята идея не е добра, напротив. Дори съм решил, скоро като завърша проекта, да направя и твоя алгоритъм, винаги мога да направя отделен скеч, като сменя само алгоритъма на навигацията, друго се запазва.

Седем разряда са напълно достатъчни за точността, но аз малко прекалявам и се престаравам. Осмия разряд от координатите от 0-9 дава толеранс 1 м. Точността ми в този разряд бяга с до 3 единици или 30 см. Това говоря при запис на координатите в паметта. Такава точност при следване на маршрут не ми е нужна и бих казал практически невъзможна, защото има много други фактори, които ще пречат на такава точност, главния от които е GPS-приемника.

При него 8-ми разряд варира с до 10-15 единици и ако няма хубав сигнал дори повече - играе си там постоянно се мени. Мислих да му слагам медиантен филтър и може би ще му сложа, защото този филтър елиминира отскоците и не дава средна средна аритметична стойност, като сбор от някщолко, а отделя средното показание от няколкото, колкото задам - примерно 5-6, ако настроя GPS-a на 5-6 Hz опресняване.

Та с тая любителска техника не мога да постигна кой знае какво. Има много добри приемници, но са скъпи и у нас трудно се намират, а и не виждам чак такъв смисъл, да гоня сантиметри точност. Дори в тази връзка съм предвидил умишлено намаляване на точността с около 2 м. Т.е. при наближаване на отправната точка, ако разчитам на навигатора да мина през нея, елеминирайки вълни, вятър и т.н., няма да се случи често и ще подмина точката, тогава навигатора ще дава команда назад, кръгове и т.н., което е недопустимо. Зетова в скеча съм задал условие, ако текущото разстояние е по-малко или равно на 2 метра, се зарежда следващата запеметена точка от маршрута за следване. Два метра това е радиус, т.е. трябва да улуча кръг с размер 4 м, изпусна ли го почва кръжене. 

Иначе има софтуерно решение на проблема с точността на 8-мия разряд, но все още не съм го осмислил. Функция, която свежда грешката на 8-мия разряд до 9.5, т.е. остава погрешност само 0,5 или това е да го наречем 5 см. Ако имаш интерес погледни и коментирай:

http://arduino.ru/forum/programmirovanie/etyudy-dlya-nachinayushchikh-tip-float-kak-ne-utonut#comment-341969

//   Возведение числа f в целую, неотрицательную степень n
//
float iPow(const float f, const int n) {
   if (n <= 0) return 1;
   if (n == 1) return f;
   float f2 = iPow(f, n / 2);
   f2 *= f2;
   return (n & 1) ? f2 * f : f2; // если n - нечётно, надо ещё раз домножить на f
}

[nizo]

EDM, ако отвориш документацията:

https://www.arduino.cc/reference/en/language/variables/data-types/double/

пише, че double е обикновенно 4 байта, но Arduino Due има 8 байтов double с такава конструкция.

[EDM electronics]

Аз мисля, че това зависи от компилатора. Т.е. така се компилира, че едно 4-байтово число се чете на 4 такта по 1 байт. Double трябва да го чете на 8 такта на генератора, защото процесора е 8-битов. Ардуино Дуо или сега съм поръчал Мега са също 8-битови. Ама щом казват, може и да е така, не съм задълбавал.

Като правя пробите с GPS-а, последните две цифри от координатите на числото float с точност до 8 знака или 6 след запетаята, та тия 5-ти и 6-ти ми дават сантиметрите 0-99.
Направих друга функция за запис във външна EEPROM на число float на "С" и ми дава голяма точност, закръгля в рамките на 1-ца. Това ще рече 1 см точност - напълно ненужно е да търся повече. А загубих доста време, докато направя коректен запис и четене в паметта. Сега е ОК.
GPS-а и той играе леко, точността има дрейф от порядъка на 20-30 см. Ако някой прояви интерес, ще публикувам функциите - запис и четене.

Неточността обаче ще се появи не от електрониката, защото при мен тя е без компромис, а от това че GPS-навигацията за граждански цели се прави умишлено с код-дрейф на времената на приемане, за да варира постоянно точността, за де не се ползва за военни цели или терористични актове в рамките на 2-10 м. Т.е. запомням днес една точа, а утре тя е на 10 м вляво, вдругиден е на 7 м северозапад. Затова и има специални точни геодезични GPS-приемници, но те се водят на отчет, там няма дрейф.

Та малко дълго стана, но float е пре-пре-пре достатъчен за моя проект. Наното го запълних на 80% и не знам дали ще ми стигне памет. Имам още работа по компаса. Най-много памет ми отне дисплея, имам 25 екран-менюта.

[EDM electronics]

Не ти трябват тангенси и разни сложни сметки.

Има точка 1 с координати X1 и Y1. Имаш т. 2 с X2 и Y2. Съотношението (x2 - x1) / (y2 - y1) ти дава ... абе наклона на правата която следва да поддържаш.
Тръгваш от т.1, стигаш до точка n  и смяташ това съотношение - (Xn - X1) / (Yn - Y1). Ако полученото число е по-голямо, значи се движиш повече по X отколкото трябва, т.е. движиш се по-полегато, трябва да завиеш леко наляво. Правиш още едно замерване след определено време и виждаш колко е числото. Пак правиш корекциите ако е необходимо.

Нещата малко трябва да се усложнят в зависимост от това в каква посока се движиш, вероятно ще трябват 2 или дори 4 алогритъма в зависимост в кой квадрант става придвижването, но общо-всето се променят само знаците + и -. т.е. в единия случай завоя ще трябва да е наляво, в другия - надясно.

Ще е хубаво да сложиш един PID който да прави корекцията с малко изпреварване, щото иначе ще кривуличиш леко. Прилагам един PID регулатор който го писах сам за Данфоските контролери. Входа е 0-1000, изхода е 0-1000, изчислява I и D коефициентите въз основа на измерванията до 50 стъпки назад...

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

Много време се играх с компаса - датчика за абсолютна ориентация. Ползвам BN 055 на BOSCH и мисля, че няма по-добро засега на любителско ниво. .
Датчика има вграден 32-битов STM-контролер и реално изкарва готови данни по шина I2C. Лесно се свързва и има автоматична настройка по всички оси и компаса, която става непрекъснато, независимо от релефа на местността и мястото на земното кълбо, дава много точни и филтрирани - спокойни стойности. Проблема беше, че като се пусне първоначално без да се разклати не може да се самонастрои. Затова си играх да правя код, с който снемам при ръчна настройка данни за корекциите, това са 8 променливи и или се записват в ЕЕПРОМ и се четат при всеки старт, или сега ги направих на константи във флаша. Така още при запускане датчика е грубо настроен, а само след леко разклащане контролера му си знае работа и точността му "кове".

Оказа се обаче, че без ПИД-регулатор не мога да мина. В противен случай руля ми ще се движи постоянно, при най-малката разлика на посоката от маршрута и ще има зигзагообразно движение.
Не бях запознат с ПИД-а, но след малко теория и стана ясно, че е много ценна "вещ" и няма как да мина без нея. Долу давам един клип с превод от английски за принципа на работа - отлично обяснено.

juliang поразгледах твоя код на ПИД, но не се ориентирам лесно. Пишеш дълги имена на променливите и се обърквам. Няма и коментари кода. Интересува ме, кои са 3-те константи, които задаваш за Пропорционалната /настояща/, Интегралната /минала/ и Дефиренциална /бъдеща/ регулировка?

Ще търся някакъв готов код, няма да откривам топлата вода на ПИД. Трябва ми вход-изход и регулировка на 3-те параметъра. Дори ще ги изведа на енкодера и дисплея със запис в ЕЕПРОМ, да може да се програмират ръчно.

<a href="https://www.youtube.com/v/NbEhtZlSa6A&amp;feature=emb_logo" target="_blank" class="new_win">https://www.youtube.com/v/NbEhtZlSa6A&amp;feature=emb_logo</a>

[Altium]

На какъв компилатор пишеш STM-a?

[valex]

https://github.com/br3ttb/Arduino-PID-Library
https://playground.arduino.cc/Code/PIDLibrary/
https://hackaday.com/2018/04/14/pid-control-with-arduino/
Може да помогнат.

[dmitarp]

Дори и да ползваш готов код, гореспоменатите коефиценти, трябва сам да си ги определиш защото зависят само от твоята система. Затова ориентирай се да се запознаеш малко с информация за настройка на ПИД регулаторите.

[valex]

Мисля, че има самoнастройващ се PID.

[juliang]

Мисля, че има самoнастройващ се PID.

1. Няма да мустигне паметта за толкоз код
2. Самия процес на адаптация на ПИД-а изисква системата да работи, т.е. да е в движение


Теория на ПИД-а:

Имаш входна стойност (примерно в диапазона 0-1000)
Имаш желана стойност (0-1000)
Имаш изходна стойност (0-1000)

Съвсем примерно искаш температурата да ти е 200 градуса (или поската на движение да е 200 градуса). Имаш последни 4 измерени данни 150, 180, 200, 210.

Пропорционалния коефициент е отклонението на последното измерване спрямо заданието. Т.е. ще е 200 - 210 = -10.
Интегралния коефициент ще е 200 - ((150+180+200+210) /4) или +15
Диференциалния коефициент ще е (150-210)/4 или -15

Какво казват тези числа:
Пропорционалния казва "вече си прехвърлил заданието, ще трябва да намалиш изходната стойност".
Интегралния казва "да, ама през последните 4 измервания в 3 от случаите си бил под заданието, така че ще трябва да увеличиш изхода, за да достигнеш заданието по-бързо"
Диференциалния казва "скоростта на нарастването ти е голяма, ще трябва да намалиш изхода, за да не прескочиш заданието.

Всеки от тези 3 коефициента се умножава по един свой си множител (Кп, Ки, Кд), който определя колко "важен" е дадения коефцициент. Точно намирането на тези множители е "адаптацията" на контролера, и зависи изключително от инертността на системата която се управлява.

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

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

Диференцилания коефициент работи в общия случай наобратно на предните два. Когато пропроционалния и интегралния крещят "ДАВАЙ, ДАВЙ НАГОРЕ!!!", то диференциалния се влияе от скоростта на нарастване а не от абсолютна стойност на отклонението, и се явява спирачка на стремежите да се достигне бързо (и вследствие на това да се прескочи неконтролируемо) заданието.

Ако и трите множителя в примера са 1, то на изхода ще получиш 200 + (-10 +15 -15) или 190. Т.е. системата ще усети че е прекалила, че тенденцията е към още по-голямо о*иране и ще започне да се връща.

[EDM electronics]

juliang, много добре разбираемо го обясни. 
Като ти гледам сметките, кода не е толкова труден. Ще намеря обаче някоя готова библиотека.
В момента съм вързал една малка серво-машинка, но съм и сложил закъснение и плавно набиране, да е инертна и да се движи бавно, защото реално двигателя /сервото/ ми ще трябва да се върти бавно. Ориентировъчно теглото на целия мотор с батерията ще е 6-7 кг. и резките движения на тия килограми ще разрушат бързо редуктора.

Мога да настроя грубо тоя ПИД, но няма де е в реални условия, а там нещата ще са други. Затова ще е достатъчно само да направя работещ код и да изведа 3-те константи за ръчно програмиране, чрез дисплея. Окончателната настройка на ПИД-а ще стане в реални условия - на вода, а те в домашни условия няма как да се случат с пробната малка серво-машинка.

[juliang]

Тук можеш да видиш как работят заедно 2 ПИД-а - един заводски и един който съм го писал аз:
https://www.youtube.com/watch?v=D37f_ZUM8Kg

Вдясно малкия екран е един KS40, който има задание да държи налягане 3.2 бара в един парен котел. В зависимост от промяната на налягането увеличава или намалява мощността на горелката (зеленото число на втория ред - 0-100%).
Вляво е един данфоски контролер, който пък мери налягането на горивото (биогаз) което постъпва в горелката и въз основа на това управлява едновременно 2 газодувки, които доставят газта от балона до горелката. Задачата му е да поддържа 150 милибара независимо от това колко консумира горелката (не казвайте на никой, щото по закон нямам право да дигам налягането над 100 милибара ).

Алгоритъма тук е допълнително усложнен от това, че при ниска мощност на горелката дебита на газодувките е по-голям от необходимото (газодувката не може да се сваля на по-малко от 40% от макс. си мощност), и се налага по някое време вместо две газодувки да остава да работи само една. И за да ми е още по-весело, двете газодувки са различни, т.е. имат различен дебит ...

Та... ПИД-а ще ти е необходим ако имаш голяма инертност. Инертността няма да можеш да я компенсираш с бавно движение на мотора - ще почне да осцилира. Аз имах същия проблем с тези газодувки - тръбопровода е почти 200 метра, и от момента в който газодувката се включи до момента в който налягането на газта при горелката почне да се качва закъснението е към 5 секунди... А аз нямам резервен обем горе при горелката, който да задържи налягането 5 секунди, и се налага про най-малкия намек за прромяна на налягането контролера да реагира изпреварващо. Тук се наложи да пипна кода на контролера и да уможа диференциалния коефициент още веднъж по десет, защото множител 100 не ми беше достатъчен ...  при множител на пропорционалния от порядъка на 4 или 5 диференциалния работи с множител от порядъка на 500

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

[VITAN]

++1