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

Неактивен EDM electronics

  • Global Moderator
  • Много Напреднал
  • *****
  • Публикации: 4 503
  • Z - Оркестър Вагнер
Re: GPS-автопилот - алгоритъм
« Отговор #105 -: Февруари 19, 2020, 11:04:16 am »

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


Заинатих се, че вчера не успях да опростя още формулата, да премахна корена и квадрата, и ето какво получих днес, дано не ми пишете 2-ка по математика:

Извлякох математическата формула от 3-тото уравнение на dmitarp.
Получих това при унищожаване на корена и квадратите:
d = 2 * r * 2 (phi2 – phi1 + cos (phi2 + phi1) * ( L2 – L1));

Крайно се получи това:
d = 4 * r * (phi2 – phi1 + cos (phi2 + phi1) * ( L2 – L1));

Тук давам снимка на извеждане на горното уравнение, че не знам тия корени и квадрати как се пишат от компютъра:

Неактивен mind

  • Сериозно Активен
  • ***
  • Публикации: 382
Re: GPS-автопилот - алгоритъм
« Отговор #106 -: Февруари 19, 2020, 11:59:45 am »
Как от трети ред, скочихте в четвърти?
Я заместете с прости числа да видите какво ще излезе.

Неактивен dmitarp

  • Много Напреднал
  • *****
  • Публикации: 1 294
  • Пол: Мъж
Re: GPS-автопилот - алгоритъм
« Отговор #107 -: Февруари 19, 2020, 12:06:42 pm »
За съжаление корен втори от сбор не се изчислява така. Ако искаш малко да намалиш времето, както казва julian можеш да заместиш 2*r с диаметеръра на земята, може и още малко да се направи. Но аз считам, че скоростта ти е достатъчно голяма, все пак трябва да се види и другата част за колко време ще се смята.

Неактивен valex

  • Специалист
  • Много Напреднал
  • ***
  • Публикации: 1 505
  • Пол: Мъж
  • изчислителна техника
Re: GPS-автопилот - алгоритъм
« Отговор #108 -: Февруари 19, 2020, 12:49:22 pm »
Ако се позоваваме на точността на GPSа (имаме начална gps и крайна координата при отсечка от придвижването по трасето) ще си позволя да предложа един по-простичък алгоритъм без никакви допълнителни корекции на GPS. Само X и Y като координати без да се използва привързване към компас:
По условие  разглеждаме само двукоординатна система с начална и крайна зададени точки  и права линия като оптимален път за достигане на крайна точка.
Тръгва се от началната в някаква посока (може да е и нарочно насочена към крайната точка)
1. в следващия момент отчитаме след усредняване данните от GPS и сравняваме със сметки на апроксимация на правата линия между двете точки. Получават се разлики от необходимото положение.
2. наличните разлики  dX и dY ги ползваме кадо данни вкарани в PID регулатор за пресмятане на компенсиращото въздействие в ъгъл на завъртане на руля спрямо текущото му положение (като скоростта на завъртане на руля до необходимото положение трябва да е обратнопропорционална на скоростта на движение)
3. към т1. до достигане на координатите на крайната точка с някакво приближение за точност
4. взема се следващата отсечка от въведеното трасе и към т1.
Така лодката ще се опитва да постоянно да се придържа към курс близък с правата линия между двете точки. Автоматично ще се компенсира влияние на вятър, течение, влачене на нещо и други странични въздействия върху движението.
п.п. За по-финно управление в изчисленията на  PID регулатора трябва да участва и скоростта на движение (евентуално обороти на витлото)

Неактивен mind

  • Сериозно Активен
  • ***
  • Публикации: 382
Re: GPS-автопилот - алгоритъм
« Отговор #109 -: Февруари 19, 2020, 12:50:01 pm »
... ъгълът трябва да бъде отчетен с точност около 1/r=1x10-5 в градуси или 1.6x10-7 в радиани. Тези числа са на границата на точност на 32 битово число с плаваща запетая (float), според мен е добре да се работи с 64 битово число (double).
dmitarp, аз чета отгоре-отгоре, но ако може да поясните нещо за мен.
Защо се налага да изчислявате с абсолютната стойност на ъгъла? За даден ареал, най-голямото отклонение е последните 4-5 цифри, или за голяма точност 8 бита са достатъчни ::) - може и да бъркам.

Не съвсем точен пример, но:
 43.418968, и.д. = 24.626770, и втора точка с.ш. = 43.419409, и.д.=24.626914.
Вижда се, че първите 4 цифри не се променят за целият ареал, защо е нужно да се "щракат" всеки път?

Неактивен dmitarp

  • Много Напреднал
  • *****
  • Публикации: 1 294
  • Пол: Мъж
Re: GPS-автопилот - алгоритъм
« Отговор #110 -: Февруари 19, 2020, 01:10:16 pm »
Това е възможно, но все пак сам се убеждаваш, че ъглите трябва да се отчитат с голяма точност. След това има различни подходи, но това което предлагаш изисква много сложен код, едва ли си заслужава след като контролера  има способности на точности до 1х10-6. Това нещо се прави ако искаш да смяташ със точност отвъд преките възможности на контролера. Например числото пи е сметното далеч далеч отвъд точността на който и да е компютър но за целта се прави специален софтуер. Освен това при 8 битовите контролери тази точност така или иначе се постига софтуерно, но за това се грижи компилатора. А аз смятам, без да съм специалист в това, че компилатора доста добре оптимизира смятането, аз не съм в състояние да го направя по-добро.

valex тази схема на алгоритъма е предложи и julian, според мен има един недостатък, ако първоначлно тръгнеш в грешната посока (защото поската на тръгване този алгоритъм не ти е гарантира), то компенсацията ще е трудна, и докато компенсираш траекторията, вече може да си подминала следващата точка от траекторията.

Неактивен EDM electronics

  • Global Moderator
  • Много Напреднал
  • *****
  • Публикации: 4 503
  • Z - Оркестър Вагнер
Re: GPS-автопилот - алгоритъм
« Отговор #111 -: Февруари 19, 2020, 01:35:04 pm »
Затова каква точност е нужна за следването на маршрута:
Трябва да се има предвид, че при наближаването на т.Б, ако не съвпаднат напълно свръх прецизните координати от GPS-а за ширина и дължина с 5-6 знака след запетаята, което ще е в 99,9% от случаите, лодката ще подмине точката и ще следва завой отвъд маршрута и множество неуспешни опити в кръжене около тая точка. При втория опит я улучи пак, я не, щото атмосферните условия ще са неподходящи. Затова мисля, че за нуждите на точното изчисление е нужно голяма точност и използването на 32 битово число, но после това число или трябва да се ограничи така, че точността да е 1-2 м. около т. Б, т.е. дори да мине близо до т. Б, програмата да отчете достигането й и да задейства брояч, който да смени адреса на четене от ЕЕПРОМ следващата точка, макар и реално да минава покрай нея  на 1-2 метра. Това е нужно, още повече, че тази точност 1-2 м е половината размер на лодката. Другия вариант е да се ползва това число със стотни, но да се постави условие за + и - еди колко си, да отчете достигната точка и да револвира адреса на четене от паметта.

В момента ардуиното изчислява двете формули за общо 1 mS. Като се добави останалата част от кода за управление на руля, паметта, енкодерите, дисплея, библиотеките и т.н., предполагам времето на един цикъл ще стане 2-2,5 mS. Това ще рече изчисление 400-500 пъти в секунда. Пре-пре-достатъчно.
« Последна редакция: Февруари 19, 2020, 01:56:43 pm от EDM electronics »

Неактивен Радико

  • Много Напреднал
  • *****
  • Публикации: 7 263
  • Пол: Мъж
  • Потребителя не съществува
    • http://martinov-radiko.blogspot.com/
  • Скайп: radiko1a
Re: GPS-автопилот - алгоритъм
« Отговор #112 -: Февруари 19, 2020, 02:10:55 pm »
Цитат
Трябва да се има предвид, че при наближаването на т.Б, ако не съвпаднат напълно свръх прецизните координати от GPS-а за ширина и дължина с 5-6 знака след запетаята, което ще е в 99,9% от случаите, лодката ще подмине точката и ще следва завой отвъд маршрута и множество неуспешни опити в кръжене около тая точка. 
Това, че джипиеса ще даде координати със пет знака въобще не значи, че ардуиното е длъжно да прочете и да се съобрази със всичките знаци. Можеш да му укажеш да прочете само до цели числа или до два, до три знака, до колкото искаш ти.

Неактивен EDM electronics

  • Global Moderator
  • Много Напреднал
  • *****
  • Публикации: 4 503
  • Z - Оркестър Вагнер
Re: GPS-автопилот - алгоритъм
« Отговор #113 -: Февруари 19, 2020, 02:54:05 pm »
Вместо координатите за ширина и дължина е по-добре да се следи разстоянието между т.А и т.Б. поставя се условие, когато то стане по-малко от примерно 3 м и да преминава от т. Б към т. В. Така все едно точката е с радиус 3 м. Още повече може лесно да се промени, дори като опция за програмиране от енкодера.

Неактивен Иван Димов

  • Експериментатор
  • Сериозен
  • ***
  • Публикации: 1 046
  • Пол: Мъж
  • Невъзможно нещо е да докажеш, че нещо е невъзможно
Re: GPS-автопилот - алгоритъм
« Отговор #114 -: Февруари 19, 2020, 04:56:39 pm »
   EDM electronics, следя темата отгоре-отгоре за обща култура. Като смяташ математиките, не бързай, проверявай по няколко пъти. На третия ред видях, че имаш грешка. В дробта под в.с знаменателят трябва да е 4, а ти си написал 2. По-надолу не съм гледал. Аз точно такива програми не съм правил, но мисля, че и тук важат принципите на последователност на смятането. Първо са функциите, корените, степените, после се смята умножение, деление и чак накрая събиране, изваждане. Затова трябва да се внимава и да се слагат скоби, когато изразът е по-дълъг. Защото компютърът е "прост" и не може да се сети сам как да смята. Трябва всичко да му е точно подредено за да го "преглътне". :)
п. п. Като се загледах по-надолу ... коренът не си го премахнал правилно. Опресни си математиката, явно ще ти трябва и за в бъдеще.
« Последна редакция: Февруари 19, 2020, 05:23:00 pm от Иван Димов »

Неактивен valex

  • Специалист
  • Много Напреднал
  • ***
  • Публикации: 1 505
  • Пол: Мъж
  • изчислителна техника
Re: GPS-автопилот - алгоритъм
« Отговор #115 -: Февруари 19, 2020, 08:04:20 pm »
dmitarp
Да. Пропуснал съм идеята на juliang. То е същото.
Никакъв проблем няма с ориентировката. От втората точка на GPSa можеш да се ориентираш за посоката и без компас. Ако сложиш и жироскоп съвсем ще си ориентиран :)
Проблема с управлението по-скоро не е ориентирането, а начина на отработване на ъглите за корекция на посоката при движение според мен. Там има забавяне на реакциите при промяна на посоките, които зависят от скоростта на движение, скоростта на промяна да ъгъла на руля и още други въздействащи неща. Тези въздействия трябва да се вградят в алгоритъма на регулатора за управление.
Темата е доста интересна, но за съжаление нямам време да се впусна в проби :(

Неактивен EDM electronics

  • Global Moderator
  • Много Напреднал
  • *****
  • Публикации: 4 503
  • Z - Оркестър Вагнер
Re: GPS-автопилот - алгоритъм
« Отговор #116 -: Февруари 20, 2020, 10:35:08 pm »
Изпробвах формулите на STM32F103C8T6 и се подобри бързодействието 4 пъти.
Все пак контролера работи на 72MHz и е 32 bit, а Arduino Nano 16 Mhz 8 bit.
Общото време за двете формули е под 300 uS. А този контролер има предостатъчно памет за няколко такива проекта.  И е много скъп, само 3 лв. :D
Контролера може да се разгони до 140 MHz и така времето ще се съкрати още двойно, но мисля, че не е нужно.

Освен това точността спрямо Гугал мапс сатна с разлика само 10 см на 424 м, а с Ардуиното беше 30 см. Но радиуса на земята във формулата мисля трябва да се коригира малко, да се вземе средно аритметичната стойност на радиуса към полюсите и радиуса към екватора. Тогава изчислението ще кове.

211.00 424.62
107.00 424.62
73.00 424.62
74.00  jul424.62
217.00  64.42
211.00 424.62
104.00 424.62
73.00 424.62
73.00  jul424.62
217.00  64.42
213.00 424.62
105.00 424.62
74.00 424.62
73.00  jul424.62
218.00  64.42
« Последна редакция: Февруари 20, 2020, 11:03:37 pm от EDM electronics »

Неактивен EDM electronics

  • Global Moderator
  • Много Напреднал
  • *****
  • Публикации: 4 503
  • Z - Оркестър Вагнер
Re: GPS-автопилот - алгоритъм
« Отговор #117 -: Февруари 22, 2020, 03:45:37 pm »
Озорих се малко с контролера STM32F103C8T6, докато разбера кое накъде е и как да ползва ардуинската среда, и попаднах на едно много интересно видео, в което се прави измерване на скоростта на изпълнение на програмата, а не изчислителните възможности на Ардуино с STM32F103C8T6 в различни среди на програмиране с "С". Разликата с ардуинската е 28 пъти по-висока скорост :D. Но има един коментар под видеото:
- Ассемблер всегда был быстрее. Но жизнь слишком коротка.  :D

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


Неактивен valex

  • Специалист
  • Много Напреднал
  • ***
  • Публикации: 1 505
  • Пол: Мъж
  • изчислителна техника
Re: GPS-автопилот - алгоритъм
« Отговор #118 -: Февруари 22, 2020, 05:44:59 pm »
Предимството на Arduino е наличието на библиотеки. Удобен е за първоначални опити, а после може да се пише на някакво С.
За работа в реално време е неудобен/невъзможен. Макар, че съм го ползвал едновременно с LAN, Bluetooth, WIFI, TFT display, EEPROM и още други неща.

Неактивен valex

  • Специалист
  • Много Напреднал
  • ***
  • Публикации: 1 505
  • Пол: Мъж
  • изчислителна техника
Re: GPS-автопилот - алгоритъм
« Отговор #119 -: Февруари 23, 2020, 03:07:04 pm »
За по-мощни неща с Arduino  може да се ползват платки NUCLEO
https://www.digikey.bg/products/en/development-boards-kits-programmers/evaluation-boards-embedded-mcu-dsp/786?k=NUCLEO
Имат пинове съвместими с Arduino и допълнителни. Подържат се от софтуера.
https://www.st.com/resource/en/data_brief/nucleo-f429zi.pdf
Ползвал съм NUCLEO F429ZI за проби.
А има и такива вършачки
https://www.udoo.org/udoo-x86/