Справочници, схемотехника, теория > Програмиране на микроконтролери, програматори, цифрови проекти
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 стъпки назад...
Навигация
[0] Списък на темите
Премини на пълна версия