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

[EDM electronics]

Пускам тази тема, защото няма ясно синтезирана информация в Интернет за алгоритъма на GPS-автопилота. Целта е да се изведе такъв, който да се програмира в контролер.

Основните GPS данни са ширина и дължина. Измерват се в градуси, минути и секунди и представляват отстояние на точките от екватора по хоризонтал и гринуичкия меридиан по вертикал.

Ширината представлява ъгъла от екватора посока север с положителна стойност, той образува максимално + 90 гр. и спрямо юга образува - 90 гр.

Дължината представлява ъгъла между гринуичкия меридиан посока изток и се измерва в + 180 гр. и в посока запад - 180 гр.

GPS-а дава първо ширината, после дължината!

Целта на задачата е да се намери алгоритъм - формула, който да определи разстоянието между две GPS-точки А и Б и азимута на тези точки. Азимута представлява ъгъла, който се образува между дадена GPS-точка и магнитния север - измерва се от 0-360 гр.

Има и друго понятие от магнитния север, то е географски север. Това е разликата от показанието на компаса от реалния север, което е променлива величина, появила се от изместването на полюсите от извънземните  . Да речем варира от 10-50 км всяка година. Всъщност магнитното поле на земята не е права линия север-юг, а е крива линия. Това произтича от факта, че земята не е абсолютно кръгла. Но тези аномалии също няма да влязат в търсения от мен алгоритъм, защото автопилота ще е за къси разстояния от няколко километра. Когато напредна с хиперзвука и кавитационното-плазмено торпедо  , ще усъвършенствам формулата.

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

Отсечката между т.А и т.Б представлява реално дъга и тя се изчислява по сложна формула. Мен тази дъга не ме интересува, макар и точно тя да дава реалното разстояние, т.к. автопилота ще се ползва за къси разстояния и това закръгление ще е несъществено, грешката ще е минимална. Интересува ме само правата отсечка, тя дава разстоянието между две точки. Т.е. търся по-опростен вариант на алгоритъма.

Някой, ако има идея за алгоритъма, моля да коментира, ще бъде полезно не само за мен. 

http://cadis.bg/2012/01/23/%D0%BA%D0%BE%D0%BE%D1%80%D0%B4%D0%B8%D0%BD%D0%B0%D1%82%D0%BD%D0%B8-%D1%81%D0%B8%D1%81%D1%82%D0%B5%D0%BC%D0%B8/

[montanar]

EDM electronics твърде много усложняваш ми се струва ....
Ако ти трябва обекта да се придвижи по права линия до зададени кординати от произволни такива ГПС се ползва само за обратна връзка да се снема информация къде точно се намира обекта - от тук на сетне се прави алгоритъм за управление като се взема предвид текущата позиция.
Блоково би трябвало да изглежда нещо от сорта ....
ГПС казва например Х и У кординати в текущия момент =>  смята се ъгълът под който е разположен обекта спрямо нужните кординати => определяне на реален север на базата на магнитен измерен такъв спрямо разположението на обекта => пускане на цикъл за оеднаквяване на ъгълът - например (подаване на обороти на витлото и ъгъл на руля и командата цикли докато се изравнят и продължава да цикили с цел коректировка на отклонението)
Общо взето програмирането се изразява в разбиване на задачата на логични за постигането и прости стъпкки.

Ето ти една насока на супер опростена такава играчка - има скриптове - разгледай ги и тях ..... мисля че може да се адаптира нещо от това проектче към твоята идея.
https://www.instructables.com/id/Arduino-Powered-Autonomous-Vehicle/

А тук и малко по-развито.
https://youtu.be/HMksrzmAj88

Жалко че нямам свободно време - иначе идеята е хубава дъвка за дъвкане.

[dmitarp]

Ето формулите за намиране на ъгъла, между мередиана (севера), и отсечката от две gps точки, a и b (азимута, както ти го наричаш, но това не е азимут, азимута показва височината на точката, например на слънцето).
β = 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 градуса

[EDM electronics]

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

Кажи ако знаеш каква е формулата - математическа? Дал съм реални координатни точки А и Б. Как ще определиш направлението на отсечката А-Б спрямо текущата позиция? лесно е да се каже на думи, и аз така казвам, но да се изведе формулата е решението на задачата.

Искам същото нещо, но за да стане, за да се определи ъгъла на отсечката спрямо лодката, трябва да се намери азимута на отсечката по GPS координатите, не по компаса. Компаса не може да покаже ъгъла на отсечката, той показва ъгъла на лодката спрямо севера. Знаейки азимута на отсечката от GPS и азимута на лодката спрямо севера, вадят се и се подава сигнал на руля в зависимост от разликата в двата измерени ъгъла, докато съвпаднат, т.е. докато лодката съвпадне с направлението на отсечката А-Б.

Точката А се явява текущата позиция, а т. Б отправната. След достигане до т. Б, тя вече става А, а следващата точка по реда става Б. Така че естествено ще се следи постоянно текущата позиция спрямо отправната и постоянно ще се следи азимута на направлението спрямо азимута на компаса.

Формулата коя е, формулата, не ми трябват скриптове и скечове, трябва ми математическата формула за определяне на азимута на отсечката А-Б по данните на GPS, не по компаса - той сам си генерира ъгъл /азимут/, не е нужно да се изчислява?

[juliang]

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

Има точка 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 стъпки назад...

[plcsoft]

ЕДМ ето ти малко информация за накой модули който съм ползвал за обучение
 - https://erelement.com/wireless/lora-gps-shield
това е GPS приемо-предавател който е с богат набор библиотеки. Само не знам под вода дали ще върши добра работа, но иначе работи със добра точност и чувствителност във въздушно пространство.
и това
https://erelement.com/sensors/altimu-10-v5?cPath=9& е комбиниран модул от жироскоп, акселерометър, компас, барометър също със голяма библиотека
мисля че тези два модула са ти достатъчни за да направиш какъвто си искаш GPS автопилот. С едиият следиш местоположението на машината а с другият получаваш информация за движение ,посока и дълбочина и корегираш поведението според релефа.
Дано да са ти полезни като информация. За алгоритъма на управление може да мислиш едва като уточниш какви модули ще използваш и какви параметри ще гониш с каква комуникация ще кореспондират помежду си модулите, не ти трябва предварителна формула защото като работиш с предварително уточнени координати можеш дори и карта на релефа да заложиш, ако имаш такава информация, разбира се.

[montanar]

EDM electronics абсолютна загуба на време е да почваме от АБ - ама ако ти е мерак занивмавай се - аз никога не бих тръгнал да си губя времето да пиша нещо от 0 ако има вариант да адаптирам готово решение.

И на въориса ти

[dmitarp]

Вие що се правите, че не сте виждали gps. GPS отчита само ъгли, за да се ползват вашите формули, първо трябва да се приеме, че началното положение е нулево с координати (0,0), но за да намериш координатите на втората точка спрямо първата няма как да не минеш през сложни формули. Има gps модули, на които като им зададеш нулева точка, след това могат да отчитат директно координати спрямо нея, но този модул пак извършва изчисленнията на тези координати, използвайки, "сурови данни".

[juliang]

Вие що се правите, че не сте виждали gps. GPS отчита само ъгли, за да се ползват вашите формули, първо трябва да се приеме, че началното положение е нулево с координати (0,0), но за да намериш координатите на втората точка спрямо първата няма как да не минеш през сложни формули. Има gps модули, на които като им зададеш нулева точка, след това могат да отчитат директно координати спрямо нея, но този модул пак извършва изчисленнията на тези координати, използвайки, "сурови данни".

Модула за ардуино (а и по принцип всички GPS модули) ти връща директно географска ширина и дължина: https://randomnerdtutorials.com/guide-to-neo-6m-gps-module-with-arduino/
Да, освен координатите ти връща и камара друга информация. Но самите координати се смятат на ниво "хардуер" вътре в модула.

[dmitarp]

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

[EDM electronics]

Ето формулите за намиране на ъгъла, между мередиана (севера), и отсечката от две gps точки, a и b (азимута, както ти го наричаш, но това не е азимут, азимута показва височината на точката, например на слънцето).
β = 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 градуса

Не съм точен вероятно с понятието азимут, но да речем, че е ъгъла между меридиана и той сочи север.
Точно тази формула ми трябваше. Ако може малко пояснение на формулата β = atan2(X,Y)?
β ъгъла ли е, "азимута"?
atan2 какво е?
(X,Y) какъв е тоя запис със запетая?
Останалото е ясно.

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

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

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

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

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

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

EDM electronics абсолютна загуба на време е да почваме от АБ - ама ако ти е мерак занивмавай се - аз никога не бих тръгнал да си губя времето да пиша нещо от 0 ако има вариант да адаптирам готово решение.

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

[montanar]

http://www.math.bas.bg/infos/files/2011-09-20-algoritm_geometry_7.pdf

[dmitarp]

atan2, e аркус тангес, намира ъгълът и е стандартна функция в повечето езици

[montanar]

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

[juliang]

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

Да, можеш да минеш и без компас. С Питагоровата теорема ще можеш да изчислиш разстоянието (хипотенузата) между точката до която искаш да стигнеш и точката в която си в момента. Ако при следващото засичане хипотенузата (разстоянието) е по-голямо - значи вървиш в обратна посока.
Ще върви по зиг-заг точно щото нямаш "предсказване", което може да ти осигури ПИД-а. Корекцията която ще получаваш ще означава "кривни за малко (примерно 1 секунда) руля и след това го изправи". Прави корекция, променя правата по която се движи и след това вижда дали тази права е успоредна на правата свързваща го с целта (т.е. този "коефициент на наклона" е еднакъв за двете прави). ПИД-а просто ще дава командата за корекция малко по-рано. Дълго е да ти го обясня, виж някое видео в тубата за това как работи този контролер. Най-грубо ти трябва черната линия. Червената е "прекалено нервно управление", а синята е "прекалено бавна реакция"

Представи си че караш кола. На пътя се появява препятствие. Ако реагираш прекалено бързо и рязко колата ще завие преди препятствието, но ще се насочиш към канавката и ще трябва да връщаш траекторията, и ако е пак толкова бързо, ще отидеш към другата канавка и пак, и пак... Ако пък си прекалено бавен... колата няма да завие достатъчно бързо и ще се удариш. Трябва ти премерена реакция, която да предвиди колко да завъртиш волана така, че хем да избягаш препятствието, хем да не отидеш в канавката...
Задачата се усложнява и от това, че реакцията на лодката не е мигновена - нито при започване на завоя, нито при завършването му. Ако няма ПИД, лодката ще продължи да завива след маневрата, и следващаат корекция ще трябва да я връща обратно, но и там ще "предобри" работата и ще се получи като червената линия.
Харакеристиката на ПИД-а се определя от 3 коефициента и 2 времена, които описват неговото поведение и тези 5 числа се подбират опитно за всяко едно приложение.

1. Пропорционален коефициент - най-грубо "при 1 градус отклонение от курса завърти руля на еди-колко си градуса". Можеш да искаш руля да се завърти на 1 градус, може и на 20 градуса - зависи от размера на руля, скоростта и от желанието ти колко бързо да се извърши корекцията. Той реално е основния коефициент, определящ размера и посоката на въздействие.
2. Интегрален коефициент - той помага да се коригират малки отклонения, коитопроп. коефициент не хваща. Най-грубо "последните 10 пъти отклонението беше +0.1 градуса. Малко е за корекция по проп. коеф., но щом 10 пъти все сме наляво, може би е добра идея да "чукнем" мъничко надясно?"
3. Диференциаен коефициент - най-грубо "в моменат вървим под ъгъл който е грешен с +10 градуса. предния път вървяхме с курс който е грешен с +20 градуса. Все още сме на "+" (пропорционалния коефициент все още крещи като обезумял "ДАВАЙ ОЩЕ В "-") , но дали не е време да го озаптим малко с един мъничък "+", щото ако така продължаваме, като стигнем "0", ще я подминем и ще трябва да се връщаме... (случая с червената линия)"
Двете времена са съответно за интегралния и диференциалния коефициент. Те указват колко време назад да се "следи историята на пътуването" и да се види как е протичал процеса, и съответно дали не е добре да се предприемат някакви изпреварващи или доуточняващи корекции.