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

[valex]

Не знам доколко по-точно  от компас  може да се управлява ако един вид се 'усевери' текуща позиция на лодката и от там нататък да се работи с дани от жироскопа? Естествено с переодични корекции от компаса.

[EDM electronics]

Формулите за азимута ти ги дадох по напред:
β = 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]

atan(), функция винаги има за аргумент (това, което е в скоби) отношения на две числа. В компютърните езици, има два начина на представяне на функцията atan, единият начин е atan(X/Y), но при този начин ако Y е нула и ще ти даде грешка или NaN. Затова има второ представяне на функцията като atan2(X,Y). Тук запетаята отделя двете числа. Освен това резултата от функцията се получава в радиани, а не в градуси. Въобще в компютрите тригонометричните функции работят само с радиани.Така че, ъглите на с.ш. и и.д ако се получават в градуси трябва да се превърнат в радиани. Превръщането от радиани в градуси става с формулата:
β =β*180/π, π = 3.14159265358979.
1/tg е различно от atan.

[EDM electronics]

Помислих, че е математическо уравнение, а не програмна функция, понеже я няма IDE.
Сега разбрах, че функцията е от "С", надявам се ще работи с ардуинския компилатор. Ще направя проба.

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

[dmitarp]

Не знам доколко по-точно  от компас  може да се управлява ако един вид се 'усевери' текуща позиция на лодката и от там нататък да се работи с дани от жироскопа? Естествено с переодични корекции от компаса.

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

[valex]

Практически ако си знаеш позицията и имаш видимост до някаква точкам на която знаеш координатите може с изчисления да определиш ъгъла на свързващата права спрямо севера и като се насочи лодката в тази посока да запишеш този ъгъл. Това вече ти определя 'севера' и от там нататък с жирото. Не ти трябва компас!

[dmitarp]

Има много начини да определиш положението си, но трябва да се съобразиш и със задачата, и затова тези методи не са приложими. Тук става на въпрос за малък съд, няма как там да поставяш бинокли и разни други уреди. Единственото реално приложимо са магнитометър, жироскоп и акселерометри. Всичко друго става, но не и за този случай. Иначе може и с часовник и слънце да направиш ориентация, но за целта пак трябват специални бинокли и т.н. екстри, може и по полярната звезда, но пак трябват бинокли и трябва да е тъмно и ясно. Всички тези недостатъци се решават с магнитометър.

[valex]

Понеже китайците не работят ви препоръчвам на който му са необходими малко по-скъпи , но налични датчици от тук(докато има ):
http://elimex.bg/product/127783/kit-k2008-gy-521-zhiroskop-akselerometar.html
http://elimex.bg/product/138062/kit-k2051-zhiroskop-akselerometar-magnitno-pole-i-nalyagane.html
http://elimex.bg/product/138081/kit-k2080-akselerometar-za-uno-mma8452.html
http://elimex.bg/product/138083/kit-k2082-kompas-i-akselerometar-lsm303dlhc.html
http://elimex.bg/product/138082/kit-k2081-zhiroskop-tsifrov-i-aglomer-l3gd20.html

[EDM electronics]

Няма смисъл от нестандартни решения. Просто трябва да се направи компас. Аз залагам на комбинирания датчик, който посочих през 2-3 поста нагоре. При него акселерометъра прави някаква софтуерна корекция на магнитометъра и той вече работи коректно. Явно няма резки отскоци, докато се успокои акселерометъра. Премахва и някаква грешка, която се натрупва. После има и някаква самокалибровка при всяко включване. Премахва се и въздействието на компаса при завъртане по оста Y. Поне така казват, не съм го пробвал лично.

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

Жироскопа показва движение по 3-те оси, но няма как да определи севера. Натрупва и грешка, и там има проблем. Ако с включването му се фиксира север, не се знае след колко оборота и натрупана грешка ще го държи тоя север. Комбинирания датчик явно ползва жироскопа при компенсация на компаса, при завъртане по оста Y, да не влияе на оста Х-компаса.

[EDM electronics]

Тук намерих още два КОМПЕНСАЦИОННИ датчика с много добри отзиви, точност 0,1 гр.:
CMPS10 - Tilt Compensated Compass Module
http://www.robot-electronics.co.uk/htm/cmps10doc.htm


Tilt Compensated Magnetic Compass (CMPS12)
https://www.robotshop.com/en/tilt-compensated-magnetic-compass-cmps12.html

И двата датчика са направени с чип на Bosch BNO055 - този който посочих, че ще ползвам:
https://store.comet.bg/Catalogue/Product/50207/

https://www.alibaba.com/product-detail/Three-axis-magnetometer-3-axis-accelerometer_60474912863.html

[dmitarp]

"The output of the three sensors measuring x, y and z components of the magnetic field, together with the pitch and roll are used to calculate the bearing, each of these components are also made available in there raw form."

Това подчертаното е това, което търсиш и което ти наричаш азимут. Bearing  е ъгълът между твоето направление (определено от GPS точките) и севера.

[EDM electronics]

Проверих по предложението - съкратената формула на juliang, няма изменение на бързодействието по третата съкратена формула на dmitarp.
Днес цял ден правих опити да съкратя корен квадратен с 2-ра степен на уравнението, но няма как да се получи с цел опростяване.
Долу следват: време в микросекунди и метри. Всички примери работят еднакво.

352.00 424.86
260.00 424.86
260.00  jul424.86
604.00 424.86
348.00 424.86
264.00 424.86
264.00  jul424.86
612.00 424.86
348.00 424.86
260.00 424.86
256.00  jul424.86
612.00 424.86
348.00 424.86
256.00 424.86
256.00  jul424.86

[EDM electronics]

Пробвах и формулата за азимута, но нещо не се получава. Не съм сигурен дали ардуиното приема функцията atan2(); Дава някаква грешка.

 

Код: [Маркирай кода]

// β - азимут в градуси
  // β = atan2(X,Y)
  // X = cos θb * ∆L
  // Y = cos θa * sin θb – sin θa * cos θb.
  // β = atan2(cos θb * ∆L,cos θa * sin θb – sin θa * cos θb)
  time1 = micros();
  double B = atan2(cos (phi2) * (l2 - l1), cos (phi1) * sin (phi2) – sin (phi1) * cos (phi2))  * pi / 180;
  time1 = micros() - time1;
  Serial.print(time1);
  Serial.print ("  ");
  Serial.print(B);
  Serial.println();

[dmitarp]

Имаш проблем във минуса пред синуса,  – sin (phi1) * cos (phi2))  * pi / 180; нещо много дълъг ми се вижда този минус не знам как си го написал, при мен също даваше грешка при компилирането, след като смених минуса стана всичко  добре.

double B = atan2(cos (phi2) * (l2 - l1), cos (phi1) * sin (phi2) - sin (phi1) * cos (phi2)) * 180 / pi;

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

[EDM electronics]

Имаш проблем във минуса пред синуса,  – sin (phi1) * cos (phi2))  * pi / 180; нещо много дълъг ми се вижда този минус не знам как си го написал, при мен също даваше грешка при компилирането, след като смених минуса стана всичко  добре.

double B = atan2(cos (phi2) * (l2 - l1), cos (phi1) * sin (phi2) - sin (phi1) * cos (phi2)) * 180 / pi;

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

Да, да, току що и аз го открих. Раздробих формулата и го открих. Този минус съм го копирал от поста ти и той е всъщност тире. Сега нещата работят коректно, оправил съм и градусите, давам скеча:

Код: [Маркирай кода]

  // β - азимут в градуси
  // β = atan2(X,Y)
  // X = cos θb * ∆L
  // Y = cos θa * sin θb – sin θa * cos θb.
  // β = atan2(cos θb * ∆L,cos θa * sin θb – sin θa * cos θb)
  time1 = micros();
//  double X = cos (phi2) * (l2 - l1);
//  double Y = cos (phi1) * sin (phi2) - sin (phi1) * cos (phi2);
  double B = atan2(cos (phi2) * (l2 - l1), cos (phi1) * sin (phi2) - sin (phi1) * cos (phi2));
  B = B * 180 / pi;
  time1 = micros() - time1;
  Serial.print(time1);
  Serial.print ("  ");
  Serial.print(B);
  Serial.println();
Ето и данните от сериал-порта, показва 64,39 гр.:
608.00 424.86
352.00 424.86
256.00 424.86
240.00  jul424.86
704.00  64.39
608.00 424.86
352.00 424.86
256.00 424.86
256.00  jul424.86
688.00  64.39
624.00 424.86
352.00 424.86
256.00 424.86
256.00  jul424.86
704.00  64.39