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

Неактивен EDM electronics

  • Global Moderator
  • Много Напреднал
  • *****
  • Публикации: 2 101
Re: GPS-автопилот - алгоритъм
« Отговор #90 -: Февруари 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

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

Неактивен juliang

  • Главен инквизитор
  • Много Напреднал
  • ***
  • Публикации: 2 774
Re: GPS-автопилот - алгоритъм
« Отговор #92 -: Февруари 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 101
Re: GPS-автопилот - алгоритъм
« Отговор #93 -: Февруари 17, 2020, 09:38:39 pm »
Като излезе вълна и почне да ме лашка ляво-дясно, мести ме сравнително лесно, защото лодката ми е с много плитко газене, чудя се дали алгоритъма на juliang ще се справи ли с усредняването на стойност, вероятно направление?

Неактивен dmitarp

  • Стабилен
  • ****
  • Публикации: 783
  • Пол: Мъж
Re: GPS-автопилот - алгоритъм
« Отговор #94 -: Февруари 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 101
Re: GPS-автопилот - алгоритъм
« Отговор #95 -: Февруари 17, 2020, 10:36:17 pm »
Не само не е необходимо, но и вредно. Дори да измерва бързо, ако това бързо са отрази на руля, той ще полудее - шум, разход на батерия. По принцип смятам руля да не се движи пропорционално на отклонението от направлението, а в граници и така не е необходимо и бързо измерване и той няма да работи постоянно. Това вероятно ще доведе до зиг-заг, но колко да се тия граници ще се установява опитно.

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

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

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

Неактивен EDM electronics

  • Global Moderator
  • Много Напреднал
  • *****
  • Публикации: 2 101
Re: GPS-автопилот - алгоритъм
« Отговор #96 -: Февруари 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

  • Главен инквизитор
  • Много Напреднал
  • ***
  • Публикации: 2 774
Re: GPS-автопилот - алгоритъм
« Отговор #97 -: Февруари 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 и той взима решение дали и с колко да се промени съответната стойност.

Неактивен valex

  • Специалист
  • Много Напреднал
  • ***
  • Публикации: 1 254
  • Пол: Мъж
  • изчислителна техника
Re: GPS-автопилот - алгоритъм
« Отговор #98 -: Февруари 18, 2020, 09:01:29 am »
Не знам доколко по-точно  от компас  може да се управлява ако един вид се 'усевери' текуща позиция на лодката и от там нататък да се работи с дани от жироскопа? Естествено с переодични корекции от компаса.

Неактивен EDM electronics

  • Global Moderator
  • Много Напреднал
  • *****
  • Публикации: 2 101
Re: GPS-автопилот - алгоритъм
« Отговор #99 -: Февруари 18, 2020, 10:22:27 am »
Формулите за азимута ти ги дадох по напред:
β = 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
dmitarp, искам да изведа формулата за азимута и да я приложа в твоя скеч, да видим как ще работи, обаче нещо не ми е ясен записа на тази формула:

β = atan2(X,Y)

Тези са ясни:
β е ъгъла от 0-360 гр. - азимута
atan2 е аркотангенса или 1/tg
X = cos θb * ∆L
Y = cos θa * sin θb – sin θa * cos θb.

Това е неясното:
, ей тая запетая какво означава?
Не ми е ясна функцията на запетаята в математическо уравнение.

Тук има малко теория по въпроса с формулите за ъгъла: http://www.movable-type.co.uk/scripts/latlong.html

формулата за азимута-ъгъла:   θ = atan2 (sin Δλ ⋅ cos φ 2 , cos φ 1 ⋅ sin φ 2 - sin φ 1 ⋅ cos φ 2 ⋅ cos Δλ)
- и отново има запетая в уравнението!

където: φ 1 , λ 1 е началната точка, φ 2 , λ 2 крайна точка ( Δλ е разликата в дължината)

Това ли е представянето на формулата със запетаята , ?


Неактивен dmitarp

  • Стабилен
  • ****
  • Публикации: 783
  • Пол: Мъж
Re: GPS-автопилот - алгоритъм
« Отговор #100 -: Февруари 18, 2020, 10:55:59 am »
atan(), функция винаги има за аргумент (това, което е в скоби) отношения на две числа. В компютърните езици, има два начина на представяне на функцията atan, единият начин е atan(X/Y), но при този начин ако Y е нула и ще ти даде грешка или NaN. Затова има второ представяне на функцията като atan2(X,Y). Тук запетаята отделя двете числа. Освен това резултата от функцията се получава в радиани, а не в градуси. Въобще в компютрите тригонометричните функции работят само с радиани.Така че, ъглите на с.ш. и и.д ако се получават в градуси трябва да се превърнат в радиани. Превръщането от радиани в градуси става с формулата:
β =β*180/π, π = 3.14159265358979.
1/tg е различно от atan.

Неактивен EDM electronics

  • Global Moderator
  • Много Напреднал
  • *****
  • Публикации: 2 101
Re: GPS-автопилот - алгоритъм
« Отговор #101 -: Февруари 18, 2020, 11:49:20 am »
Помислих, че е математическо уравнение, а не програмна функция, понеже я няма IDE.
Сега разбрах, че функцията е от "С", надявам се ще работи с ардуинския компилатор. Ще направя проба.

Видях в програмата, че преобразуваш градусите в радиани и формулата е в радиани.
Ще имам и това превръщане в предвид β =β*180/π, π = 3.14159265358979.

Неактивен dmitarp

  • Стабилен
  • ****
  • Публикации: 783
  • Пол: Мъж
Re: GPS-автопилот - алгоритъм
« Отговор #102 -: Февруари 18, 2020, 01:02:50 pm »
Не знам доколко по-точно  от компас  може да се управлява ако един вид се 'усевери' текуща позиция на лодката и от там нататък да се работи с дани от жироскопа? Естествено с переодични корекции от компаса.
Да за мен това е добра идея, магнитометъра да служи само за начално определяне на позицията и след това да се използва жироскоп. Но жироскопа има един недостатък, много е муден, за това бих го сдвоил с още един източник на посока, който е бърз, а именно два акселерометъра, на кърмата и на носа на лодката, на точно определено разстояние един от друг. Като се знае разстоянието и разликата от показанията на акселерометрите може да се определи ъгъла на завъртане.
« Последна редакция: Февруари 18, 2020, 01:25:51 pm от dmitarp »

Неактивен valex

  • Специалист
  • Много Напреднал
  • ***
  • Публикации: 1 254
  • Пол: Мъж
  • изчислителна техника
Re: GPS-автопилот - алгоритъм
« Отговор #103 -: Февруари 18, 2020, 01:41:15 pm »
Практически ако си знаеш позицията и имаш видимост до някаква точкам на която знаеш координатите може с изчисления да определиш ъгъла на свързващата права спрямо севера и като се насочи лодката в тази посока да запишеш този ъгъл. Това вече ти определя 'севера' и от там нататък с жирото. Не ти трябва компас!

Неактивен van

  • Стабилен
  • ****
  • Публикации: 536
Re: GPS-автопилот - алгоритъм
« Отговор #104 -: Февруари 18, 2020, 02:32:08 pm »

Секстант от лат. sextans е уред за измерване на ъгъла между Слънцето (или друга звезда) и точка от хоризонта или за измерване на хоризонтален ъгъл между два обекта (най-често навигационни ориентири). Името произлиза от латинската дума за "една шеста" - sextāns, тъй като скалата на секстанта е с дължина една шеста от кръга - 60 градуса. С него обаче могат да се измерват ъгли до 120 градуса.

Секстантът се използва предимно като навигационен инструмент за определяне на географските координати на наблюдателя чрез измерване на ъгловата височина на различни небесни тела (Слънцето и някои звезди например Сириус −17° в даден момент (астрономическа навигация). В миналото е използван широко в корабоплаването, но след появата на по-модерни методи за навигация

(включително GPS), на секстанта е отредена ролята на запасен инструмент, който не се нуждае от електричество.


мисля камерата на телефона може да го измери това без GPS и без Kompas само точен часовник точно време и секстант с камерата на телефона и малко оптика пред нея секстант.