Справочници, схемотехника, теория > Програмиране на микроконтролери, програматори, цифрови проекти
GPS-автопилот - алгоритъм
EDM electronics:
juliang, предлагаш алтернативен вариант на алгоритъм, но ми се струва сатна по-сложен с всички тези обяснения за работа на тоя регулатор. Гледал съм софт на автопилот и по спомен там се работи по формулата, която даде dmitarp синус, косинус тангенс.
Целта на занятието е да се изчисли ъгъла от координатите на двете точки. Видно от формулата е само от 2-3 реда:
β = atan2(X,Y)
X = cos θb * sin ∆L
Y = cos θa * sin θb – sin θa * cos θb * cos ∆L.
В Ардуино има такива функции:
sin()
cos()
tan()
, но аз не съм ги ползвал досега, трябва да видя примери как работят, дори въобще съм забравил формулите за синус, косинус и тангенс, котангенс :D, та сега ще търся учебник за 3-ти клас да си доуча урока. :D
А за аркус тангес, въобще не се сещам за какво иде реч, ще ми трябва преводач.
Тоя ПИД много ми прилича на ингератор - цифров филтър. Впрочем точно това се ползва и при датчиците - компаса и GPS-а. Понеже са доста чувствителни и мърдат доста, се включват филтри, за да се получи някаква инертност. Особено това се отнася за компаса. Не съм пускал компас досега, защото още не е пристигнал, но като гледах видеата само от магнитометър е невъзможно да се направи компас, той се влияе от движение по отстта Z, затова се прави компенсация само с комбиниран датчик с акселерометър.
juliang:
PID-а се слага във веригата, може с него, може и без него. Но без него ще имаш "насам-натам".
И не, не е филтър. Макар че ще ти помогне и да филтрираш показанията - именно чрез интегралната си съставка.
Не ти трябват синуси, повярвай ми. Тези сметки са много ресурсоемки за едно ардуино.
Тръгваш от точка с координати 10,10, трябва да стигнеш до точка с координати 1000, 1000. По моята сметка коефициента се получава 990 / 990 = 1
Първото ти измерване ти дава координати 20,22. 1000-20 е 980, 1000 - 22 е 978. Коефициента е 980/978, или 1.002. T.e. трябва ти завой в едната посока (в случая ще е надясно, но все се надявам че ще можеш да обърнеш движението на руля ако е грешно :) )
Втори измерване - дава ти координати 30,31. Съгласно резултатите от предното измерване коефициента е 1.002. Сегашното е 1000-30 =970 и 1000-31 = 969 или 970/969
=> коефициент 1.001. Той е по-малък от предишния, съответно корекцията трябва да е в обратна посока - наляво. Щото ако продължиш в тази посока, следващото измерване ще е 40,40, следващото - 50,49 и ... отиваш у канавката :)
Трето измерване ти дава 40,41. 1000-40 = 960, 1000 - 41 = 959, коефициент ... 1.001. Идеално, същия като предния, продължаваме направо.
И така си вървиш, сравнявайки две числа - коефициента от текущото измерване с това от предишното. Ако двата коефициента са равни - вървиш направо, Ако са различни - завиваш в съответната посока.
Трябва ти и една сметка все пак дали не вървиш точно в противоположната посока, но се надявам една Питагорова теорема да можеш да имплементираш :)
EDM electronics:
juliang, струва си да се пробва. Ще кача на едно ардуино дисплей е GPS на батерия и ще пробвам една разходка, де видим на практика какви коефициенти ще показва и колко ще трепери тоя коефициент. Ако се наложи да слагам и ПИД, ще те питам тогава как работи.
Но има и нещо друго:
Основното предназначение на тоя автопилот ще е електрическа котва, т.е. като запаметя дадена точка, щото искам да стоя на едно място, тогава нещата стават още по-сложни. Най-полезното на автопилота е електрическата котва. Примерно няма как да се закотвя на дълбочини над 40-50 м, става много трудоемко и времеемко. С електрическата котва става мигновено - супер модерна технология. Тогава освен руля ще трябва да се управлява и двигателя и то в реверс, защото тоя рул ще има ход само +,- 90 гр. Ще се дублира управлението и от компаса, защото освен, че трябва да ме държи в тази точка, то трябва и направлението да се същото. Примерно духа вятър - извъртам се с гръб, не да ме блъска в синусите, и двигателя трябва да ме държи в тази позиция. Това е по-трудната задача, макар че ще се ползват пак същите данни, като при първия вариант, просто управлението на руля и GPS-а ще е по-сложно. Примерно, ако се намирам на повече от 3-4 метра от точката или ъгъла на позицията е по-голям от 20 гр., двигателя работи с някаква средна скорост, ако тези показатели са по-малки - по-малка скорост и т.н. това вече опитно ще се установи колко точно. Отделно трябва да се съблюдава реверса на мотора, кога да компенсира само с руля, кога и с реверс на двигателя, но с по-малък ъгъл на руля.
Твоя алгоритам ще го може ли това?
juliang:
Това не можеш да го постигнеш с GPS. Точността на този уред е 3-5 метра (умишлено е въведена погрешност за да не се ползва за насочване на ракети или други високоточни оръжия). Няма да знаеш дали си мръднал с 1 метър или не. По-лошия вариант е че ГПС-а може да те мести насам-натам в радиус няколко метра въпреки че не е необходимо, просто защото грешката също е променлива величина.
Да, вероятно ще може да те държи в някаква зона с радиус 10 метра, но на повече не се надявай. За посоката на лодката ти пък ГПС-а хептен не може да се оправи - там ще ти трябва вече компас.
Вече самия алгоритъм кое как да се ползва ще е малко по-сложен. Дста по-сложен всъщност... :)
EDM electronics:
https://akvasport.com/product/10885/el-motor-minn-kota-riptide-st-55-i-pilot-no-foot-pedal.html
Отдавна се продават такива неща. И котвата работи добре и маршрута. Ползвал съм и знам.
Котвата държи на 3-4 м толеранс, това не е много. А направлението е безкомпромисно. Дори има на дистанционното бутон за добавяне и изваждане на 10 гр. и можеш да си променяш направлението много фино. Мисля, че имаше и такава опция за позицията, т.е. корекция на GPS-а. Като задуха вятъра леко измества, но двигателя коригира най-много с 3-4 м грешка, не е повече. Голяма ще бъде грешката ако се сравняват големи отстояния между 2 точки. А тук те са минимални.
Алгоритъма на котвата няма да е много по-сложен. Ще се ползват абсолютно същите данни от GPS-а и компас разбира се. Нали той ще определя позицията. Алгоритъма ще е сложен по отношение управлението на руля, двигателя и реверса. Но колко да е сложен при положение, че частта с датчиците работи перфектно? Ако на 3 магарета плявата не можем да разделим. :)
Затова твоя вариант е неприложим в моя случай. Ще ми трябва основно азимута от GPS - горната формула да се окаже имане, но ще сравня с някои готови проекти.
Навигация
[0] Списък на темите
Премини на пълна версия