Автор Тема: GPS-автопилот - алгоритъм  (Прочетена 20385 пъти)

Неактивен VITAN

  • Сериозен
  • ****
  • Публикации: 1 137
  • Monkey see, monkey do...
    • Monkey see, monkey do...
Re: GPS-автопилот - алгоритъм
« Отговор #75 -: Февруари 17, 2020, 02:21:04 pm »
Ти си пишеш всичко от нула, вземаш нещо на готово, само ако го разбираш на 100% и му вярваш.
Никога не си правя илюзии от готовите решения.
Смартфона не е голям, с него до работещо решение ще се стигне много по-бързо, особено при дебъгването.

Никак не настоявам на решението си, нито да съм прав. От опита си с програмиране >> проекти с Microchip и Arduino -
интуицията ми подсказва, че ще е зор. Ще иска сметки с числа с двойна точност (double). Не, че не е най-елегантно с Ардуино, но е най-бавно и трудоемко.

Хайде, забравяме. Няма какво повече. Ако искаш, тук има големи спецове: http://mcu-bg.com/mcu_site/ { даже гурута  8) }
Сигурно ги знаеш, но все пак.

Неактивен EDM electronics

  • Global Moderator
  • Много Напреднал
  • *****
  • Публикации: 2 478
Re: GPS-автопилот - алгоритъм
« Отговор #76 -: Февруари 17, 2020, 02:30:10 pm »
Ти си пишеш всичко от нула, вземаш нещо на готово, само ако го разбираш на 100% и му вярваш.
Никога не си правя илюзии от готовите решения.
Смартфона не е голям, с него до работещо решение ще се стигне много по-бързо, особено при дебъгването.

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

Хайде, забравяме. Няма какво повече. Ако искаш, тук има по-големи спецове: http://mcu-bg.com/mcu_site/ { даже гурута  8) }
Сигурно ги знаеш, но все пак.

Колега, не съм търсил помощ нито за програмиране, нито за избор на контролер, нито за електрониката, нито "знаещи" програмисти, защото съм си САМОДОСТАТЪЧЕН!
Моля ви се, не ми предлагайте помощ, която не съм ви искал!
Ме ми трябва да ме впечатлявате с "гурута", те не ме интересуват.
Моля ви, контактувайте си там с вашите гурута и не ме занимавайте с тях, много моля?


Исках само да обсъдим АЛГОРИТЪМ. Темата се изчерпа с поста на dimitarp.
Оттук насетне, ако някой иска с нещо да допринесе, по-горе бях писал. Има готов код, който чака да се преобразува в ардуински.
Но понеже съм сигурен, че такъв многознайко няма да се намери, просто мисля темата е изчерпана.
« Последна редакция: Февруари 17, 2020, 02:51:14 pm от EDM electronics »

Неактивен EDM electronics

  • Global Moderator
  • Много Напреднал
  • *****
  • Публикации: 2 478
Re: GPS-автопилот - алгоритъм
« Отговор #77 -: Февруари 17, 2020, 02:37:01 pm »
И понеже се ядосах от много дразнещи постове, отдавна исках да ви кажа една българска, по-точно сливенска поговорка, дето не я знаете, която много пасва на форумното общуване и е време да стане публично достояние:

PUTKA на площад не се EBE!
Да ви кажа ли защо?
Защото се натрупва един народ, заобгражда интересната обстановка тоя народ и от всички страни и посоки започват да се дават акъли:
- Не така бе, не така, не се прави така, махни се аз да ти покажа как стават тия работи, и то от хора дето не са си намерили PUTKA, ами я карат предимно на коляно-мотовилков механизъм.  :D

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

Неактивен dmitarp

  • Стабилен
  • ****
  • Публикации: 966
  • Пол: Мъж
Re: GPS-автопилот - алгоритъм
« Отговор #78 -: Февруари 17, 2020, 07:40:16 pm »
Така или иначе си я пуснал темата, да добавя малко и аз, защото  на мен ми е интересна. Първо малко относно точността на изчисленията. Единственият линеен размер в сферичните координати е радиуса на земята r = 6371300 m, някъде съм го срещал и като r = 6378100 m. Която и цифра да се вземи грешката е в рамките на сантиметри за разстояния до 50 метра. Другите координати са ъглови, които се получават от GPS, т.е. северната ширина и дължината. Най-краткото разстояние между две точки, винаги минава през голямата окръжност на сферата. Дължината на дъгата по голямата окръжност (т.е. разстоянието между двете точки) е равна на d = r*phi, и сега за да имаме точност от порядъка на един метър ъгълът трябва да бъде отчетен с точност около 1/r=1x10-5 в градуси или 1.6x10-7 в радиани. Тези числа са на границата на точност на 32 битово число с плаваща запетая (float), според мен е добре да се работи с 64 битово число (double). Всякакъв опит да се направи библиотека, която може да използва 16 битово число, с цел пестене на време е безсмислена, защото това ще доведе до сериозни грешки.
Сега малко за функцията haversine, която се използва за намиране на разстоянието между две точки. Haversine не е име на човек, а име на тригонометрична функция и затова е неправилно да се казва формула на haversine. Тази формула може да се опрости като се знае, че при малки разстояния разликата в ъгловите координати е малка и, че sin (θ) = θ, за малки ъгли тогава формулата става:

d = 2 * r * sqrt(pow((phi2 - phi1) / 2, 2) + cos(phi1) * cos(phi2) * pow((l2 - l1) / 2, 2))

Може да се въведе и още едно опростяване свързано с косинусите от близки ъгли, което пак важи за малки разстояния ( тук под малки разстояние разбирам от порядъка 20, до 30 км), cos(phi1) * cos(phi2) = (cos((phi2 + phi1)/2)^2. Тогава формулата ще изглежда така:

d = 2 * r * sqrt(pow((phi2 - phi1) / 2, 2) + pow(cos((phi2 + phi1)/2), 2) * pow((l2 - l1) / 2, 2)).

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

Първата група опити е свързана с разстояние 50.4 м, ширината на един магазин с координати в първата точка с.ш. = 43.418968, и.д. = 24.626770, и втора точка с.ш. = 43.419409, и.д.=24.626914.

Контролер          Време, [μs]          Разстояние, [m]
                        1          2          3          1          2          3
Due                  194      103       73         50.4    50.4      50.4
Teensy 3.6        61        31         22         50.54  50.54    50.54
Uno                  608      356       256       51.08  51.08    51.08
STM32F407ZET 67        34        25         50.4    50.4     50.4

Тук с едно 1, 2 и 3 е означено формулата която се използва, 1 е пълната формула, 2 е след първото опростяване и 3 след второто.
Няма никаква разлика във времената, ако се използва (float) вместо (double),освен при STM32F407ZET, където времената намаляват почти двойно, с изключение на третата формула, където времето остава не променено. Трябва да се има в предвид, че STM32F407ZET и Teensy 3.6 имат вградено хардуерно 32 битово FPU (float point unit).

Втората група опити е свързана с разстояние 15.79 м, ширината на една сграда с координати в първата точка с.ш. = 43.411376, и.д. = 24.627289, и втора точка с.ш. = 43.411325, и.д.=24.627470.

Контролер          Време, [μs]          Разстояние, [m]
                        1          2          3          1          2          3
Due                  192      99        70         15.68  15.68    15.68
Teensy 3.6        58        30         21        15.8    15.8      15.8
Uno                  600      352       256       15.82  15.82    15.82
STM32F407ZET 67        33        24         15.68   15.68    15.68

Координатите, както и разстоянията са взети от google map, нямам GPS модул.
Опростяването на формулата не води до загуба на точност, а времето за изчисление се намалява повече от два пъти.
От опитите се вижда, че Uno се справя много добре със задачата, изчислението отнема половин мили секунда, което означава, че до 10 милисекунди може да направиш още 15, 20 подобни изчисления. А 100 хертца sample rate е съвсем достатъчно за една лодка, дори и 50 хертца биха свършили работата. Мисля, че Uno като параметри и скорост би се справил с един автопилот, какъвто се търси.
Накрая искам да призова тези, които твърдят, че има по-лесна формула нека да я демонстрират, така както и аз, ще ми бъде много полезна и не само на мен.
Ето и кода ако има някой забележки относно измерването на времето, нека допълни, може и да не съм прав:
#define pi 3.14159265359
#define r  6371300  // radius

// coordinates of points in degree
// point 1
double phi1 = 43.411376;
double l1   = 24.627289;

// point 2
double phi2 = 43.411325;
double l2   = 24.627470;

void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600);
  // coordinates of points in radians:
  // point 1
  phi1 = phi1 * pi / 180;
  l1   = l1 * pi / 180;

  // point 2
  phi2 = phi2 * pi / 180;
  l2   = l2 * pi / 180;
}

void loop() {
  double time1;
  time1 = micros();
  // distance between points full formula
  double  d = 2 * r * asin(sqrt(pow(sin((phi2 - phi1) / 2), 2) + cos(phi1) * cos(phi2) * pow(sin((l2 - l1) / 2), 2)));
  time1 = micros() - time1;
  Serial.print(time1);
  Serial.print (" ");
  Serial.print(d);
  Serial.println();
 
  time1 = micros();
  // distance between points first approximation
  d = 2 * r * sqrt(pow((phi2 - phi1) / 2, 2) + cos(phi1) * cos(phi2) * pow((l2 - l1) / 2, 2));
  time1 = micros() - time1;
  Serial.print(time1);
  Serial.print (" ");
  Serial.print(d);
  Serial.println();

  time1 = micros();
  //distance between points second approximation
  d = 2 * r * sqrt(pow((phi2 - phi1) / 2, 2) + pow(cos((phi2 + phi1)/2), 2) * pow((l2 - l1) / 2, 2));
  time1 = micros() - time1;
  Serial.print(time1);
  Serial.print (" ");
  Serial.print(d);
  Serial.println();
 
  delay(5000);
}
« Последна редакция: Февруари 17, 2020, 08:05:47 pm от dmitarp »

Неактивен valex

  • Специалист
  • Много Напреднал
  • ***
  • Публикации: 1 285
  • Пол: Мъж
  • изчислителна техника
Re: GPS-автопилот - алгоритъм
« Отговор #79 -: Февруари 17, 2020, 08:12:06 pm »
dmitarp
Поздравления за изследването!
Пълно и точно!
Професионално!

Неактивен EDM electronics

  • Global Moderator
  • Много Напреднал
  • *****
  • Публикации: 2 478
Re: GPS-автопилот - алгоритъм
« Отговор #80 -: Февруари 17, 2020, 08:51:07 pm »
dmitarp, златен си.  :D
Вери импортант персона в автопилотите.  :D 
Благодаря много!

Много работа си свършил, чак се чувствам задължен.
Ще изпробвам скеча ти, също с примери от Гугал мапс. Щом работи с примери от Гугал, от това по-показателно здраве му кажи.
Да опростяването на формулата не дава забележима грешка. По принцип не е нужна голяма точност. Плюс-минус 1 м в голямото море няма никакво значение.

Обаче в ардуинската среда най-голямото число с плаваща запетая е 32 бита  - float. double е същото число float. Затова и измерванията ти не показват разлика с промяната му.
Щом Ардуиното може да направи изчисления със скорост 100 Хц това е предостатъчно, като се има предвид, че много добър GPS приемник работи на 10 Хц. И 5 Хц са достатъчни.

Интересно ми е как мериш бързодействието и ако се напълни паметта с останалата част от програмата, дали ще изчислява със същата скорост?
От тази формула може ли да се изведе азимута на отсечката?

Неактивен dmitarp

  • Стабилен
  • ****
  • Публикации: 966
  • Пол: Мъж
Re: GPS-автопилот - алгоритъм
« Отговор #81 -: Февруари 17, 2020, 09:00:22 pm »
Формулите за азимута ти ги дадох по напред:
β = atan2(X,Y)

X = cos θb * sin ∆L

Y = cos θa * sin θb – sin θa * cos θb * cos ∆L.

Тук θa и θb са ширините на точките a  и b съответно, а ∆L е разликата между дължините.

Ако две точки имат еднакви ширини, ягълът е 90 градуса, а ако имат еднакви дължини, ъгълът е 0 градуса
Но тук може да се направи опростяване, но малко и тази формула ще отнеме повече време. Ардуиното използва стандартен С, всички тригонометрични функции, ги има.
X = cos θb * ∆L

Y = cos θa * sin θb – sin θa * cos θb.

Но както спомена по-рано на магнитометъра трудно може да се разчита, това е голям проблем, имам няколко и въпреки калибрирането дават голямо отклонение. Исках да изпробвам на Сименс един, но така и не го купих, на него по може да се вярва, но не знам BNO055

Неактивен EDM electronics

  • Global Moderator
  • Много Напреднал
  • *****
  • Публикации: 2 478
Re: GPS-автопилот - алгоритъм
« Отговор #82 -: Февруари 17, 2020, 09:17:17 pm »
С обикновен магнитометър компас не може да стане.
Да, и аз ще разчитам на тоя датчик BNO055, но още не е пристигнал от Алиекспрес.
Добре е да разгледаш това видео с калибровката и работата.

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

<a href="https://www.youtube.com/v/LBXdo1d1ZCI" target="_blank" class="new_win">https://www.youtube.com/v/LBXdo1d1ZCI</a>

Неактивен dmitarp

  • Стабилен
  • ****
  • Публикации: 966
  • Пол: Мъж
Re: GPS-автопилот - алгоритъм
« Отговор #83 -: Февруари 17, 2020, 09:24:02 pm »
Този датчик има собствен процесор, ако не се лъжа STM32f4 и може директно да даде ориентацията, но пак използва магнитометъра, може да направи и поправка на магнитния полюс, така че да сочи географския. Когато аз го поръчах за съжаление не ми го доставиха, ами върнаха парите и след това не съм поръчвал втори път, и нямам опит с датчика но чета добри отзиви за него.

Неактивен juliang

  • Главен инквизитор
  • Много Напреднал
  • ***
  • Публикации: 3 147
Re: GPS-автопилот - алгоритъм
« Отговор #84 -: Февруари 17, 2020, 09:28:03 pm »
Ако приемем, че лодката ще се ползва на територията на България, може да се приеме че една ъглова минута в посока север-юг си е равна а една морска миля, а една ъглова минута в посока изток-запад е 0.7 от една морска миля. T.e. 1 градус север-юг е 110 км, един градус изток-запад е около 78-79 км. Вече зависи как ще получваш резултатите от ГПС модула - дали в градуси или в градуси, минути, секунди... примерно дали ще е 45.053217 градуса или 45 11' 20". Но прехвърлянето ще е много лесно.
Оттам нататък отиваш на едни цели числа, които ще ги подбереш така че да работиш с  желаната точност - дали ще е милиметър, сантиметър... И сметката ти опира до Питагор-а. Квадратен корен, вдигане на квадрат и някой синус.
Синуса също можеш да го махнеш, защото теб ти трябва "коефициент на наклона" на дадена права, или съотношението на изменение на Y спрямо изменението на Х, а не точно синуса като число. Да, ако искаш да задаваш курс като ъгъл - ще ти трябва, но ако само ще даваш начална и крайна точка - не е нужен.
Оттам ти отпада огромна библиотека с геометрия, отпадат ти работа с числа с плаваща запетая (която се емулира, не се смята хардуерно като в процесорите на компютрите).
Обаче ще имаш проблем с точността. Дори и long ще събере квадрата на 600 метра разстояние и след това ще се препълни, ако работиш в сантиметри. Да, ако е в метри ще събере 60 км, но ще имаш точност няколко метра.
Ако работиш с числа с плаваща запетая нещата вероятно ще са малко по-добри, но трябва да внимаваш изключително много при преминаването от абсолютни географски координати към разстояния. Не е изключено катастрофално да загубваш точност, защото ардуиното си работи с числа със 7-8 значещи цифри.

Неактивен EDM electronics

  • Global Moderator
  • Много Напреднал
  • *****
  • Публикации: 2 478
Re: GPS-автопилот - алгоритъм
« Отговор #85 -: Февруари 17, 2020, 09:38:39 pm »
Като излезе вълна и почне да ме лашка ляво-дясно, мести ме сравнително лесно, защото лодката ми е с много плитко газене, чудя се дали алгоритъма на juliang ще се справи ли с усредняването на стойност, вероятно направление?

Неактивен dmitarp

  • Стабилен
  • ****
  • Публикации: 966
  • Пол: Мъж
Re: GPS-автопилот - алгоритъм
« Отговор #86 -: Февруари 17, 2020, 09:44:19 pm »
Именно това 0.7 ти е косинуса от северната ширина, по точно е около 0.72, 0.75, за цялата територия на България от юг към север и тогава ще имаш допустима грешка от половин метър на 50 метра, което не е проблем защото може да се коригира. А дали ще работиш с цели числа или не точността ще е една и съща, само че с цели числа може би скоростта ще е по голяма. Тествах на Due, времето е 60 микросекунди, което е само с 10 микросекунди по-малко това с използване на формулата която предложих.Контролера се затруднява не само от тригонометричните функции, но и от корен втори, повдигане на квадрат. Ако махна косинуса и сложа дефинирано число, в опростената формула също ще получа това време, но с по-добра точност. От друга страна, ако сложа косинус от средната ширина във формулата на Юлиан, също ще постигна такава точност каквато и стандартната формула. За мен извода е един, няма как да се спести значително време за изчисление, а и за една лодка със сравнително бани реакции това не е необходимо.
« Последна редакция: Февруари 17, 2020, 10:02:58 pm от dmitarp »

Неактивен EDM electronics

  • Global Moderator
  • Много Напреднал
  • *****
  • Публикации: 2 478
Re: GPS-автопилот - алгоритъм
« Отговор #87 -: Февруари 17, 2020, 10:36:17 pm »
Не само не е необходимо, но и вредно. Дори да измерва бързо, ако това бързо са отрази на руля, той ще полудее - шум, разход на батерия. По принцип смятам руля да не се движи пропорционално на отклонението от направлението, а в граници и така не е необходимо и бързо измерване и той няма да работи постоянно. Това вероятно ще доведе до зиг-заг, но колко да се тия граници ще се установява опитно.

dmitarp, изпробвах ти скеча, но не знам защо не ми дава реален мисля резултат. Знам едно място разстоянието е горе долу около 300 м, ми го дава 1 326,24 м.

Не мога да разбера как се измерваше разстоянието с гугал-мапса или ще търся друга програма Еарт и пак ще пиша.

Иначе разбрах, мериш времето на изчисление - пускаш таймер преди уравнението и мериш след него.

Неактивен EDM electronics

  • Global Moderator
  • Много Напреднал
  • *****
  • Публикации: 2 478
Re: GPS-автопилот - алгоритъм
« Отговор #88 -: Февруари 17, 2020, 11:10:47 pm »
Направих ново измерване:
Координати в точки 1. 42.587797 и 25.933175 и 2. 42.589446 и 25.937853
С Гугал мапса дава 424,57 м с формулата на dmitarp дава 424,86 м.
Разликата е само 29 см - една подметка разстояние грешка на 425 м :D
Не е зле, никак не е зле, бачка си формулката, бачка си...   ;)

Неактивен juliang

  • Главен инквизитор
  • Много Напреднал
  • ***
  • Публикации: 3 147
Re: GPS-автопилот - алгоритъм
« Отговор #89 -: Февруари 18, 2020, 03:06:50 am »
Цитат
d = 2 * r * sqrt(pow((phi2 - phi1) / 2, 2) + pow(cos((phi2 + phi1)/2), 2) * pow((l2 - l1) / 2, 2));
От малък са ме учили да не ползвам функцията pow когато става въпрос за квадрат. Още повече че според документацията на ардуино
Цитат
pow() can be used to raise a number to a fractional power
което ми говори че вдигането на дадена степен не е реализирано с многократно умножние.
Аз бих направил нещо подобно:
Цитат
double ph2ph1 = (ph2 - ph1) /2;
double cosph = cos((phi2 + phi1) / 2);
double l2l1 = (l2 - l1) / 2;
d = 2 * r * sqrt(ph2ph1 * ph2ph1 + cosph * cosph * l2l1 * l2l1);

ще ми е интересно колко микросекунди ще се икономисат (ако изобщо се икономиса нещо :) )
А, да и ... като константа може да се укаже директно диаметъра на Земята а не радиуса, така че и едно умножение по две ще отпадне :)

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

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