Справочници, схемотехника, теория > Програмиране на микроконтролери, програматори, цифрови проекти
GPS-автопилот - алгоритъм
EDM electronics:
--- Цитат на: dmitarp в Февруари 17, 2020, 07:40:16 pm ---
d = 2 * r * sqrt(pow((phi2 - phi1) / 2, 2) + pow(cos((phi2 + phi1)/2), 2) * pow((l2 - l1) / 2, 2)).
--- Край на цитат ---
Заинатих се, че вчера не успях да опростя още формулата, да премахна корена и квадрата, и ето какво получих днес, дано не ми пишете 2-ка по математика:
Извлякох математическата формула от 3-тото уравнение на dmitarp.
Получих това при унищожаване на корена и квадратите:
d = 2 * r * 2 (phi2 – phi1 + cos (phi2 + phi1) * ( L2 – L1));
Крайно се получи това:
d = 4 * r * (phi2 – phi1 + cos (phi2 + phi1) * ( L2 – L1));
Тук давам снимка на извеждане на горното уравнение, че не знам тия корени и квадрати как се пишат от компютъра:
mind:
Как от трети ред, скочихте в четвърти?
Я заместете с прости числа да видите какво ще излезе.
dmitarp:
За съжаление корен втори от сбор не се изчислява така. Ако искаш малко да намалиш времето, както казва julian можеш да заместиш 2*r с диаметеръра на земята, може и още малко да се направи. Но аз считам, че скоростта ти е достатъчно голяма, все пак трябва да се види и другата част за колко време ще се смята.
valex:
Ако се позоваваме на точността на GPSа (имаме начална gps и крайна координата при отсечка от придвижването по трасето) ще си позволя да предложа един по-простичък алгоритъм без никакви допълнителни корекции на GPS. Само X и Y като координати без да се използва привързване към компас:
По условие разглеждаме само двукоординатна система с начална и крайна зададени точки и права линия като оптимален път за достигане на крайна точка.
Тръгва се от началната в някаква посока (може да е и нарочно насочена към крайната точка)
1. в следващия момент отчитаме след усредняване данните от GPS и сравняваме със сметки на апроксимация на правата линия между двете точки. Получават се разлики от необходимото положение.
2. наличните разлики dX и dY ги ползваме кадо данни вкарани в PID регулатор за пресмятане на компенсиращото въздействие в ъгъл на завъртане на руля спрямо текущото му положение (като скоростта на завъртане на руля до необходимото положение трябва да е обратнопропорционална на скоростта на движение)
3. към т1. до достигане на координатите на крайната точка с някакво приближение за точност
4. взема се следващата отсечка от въведеното трасе и към т1.
Така лодката ще се опитва да постоянно да се придържа към курс близък с правата линия между двете точки. Автоматично ще се компенсира влияние на вятър, течение, влачене на нещо и други странични въздействия върху движението.
п.п. За по-финно управление в изчисленията на PID регулатора трябва да участва и скоростта на движение (евентуално обороти на витлото)
mind:
--- Цитат на: dmitarp в Февруари 17, 2020, 07:40:16 pm ---... ъгълът трябва да бъде отчетен с точност около 1/r=1x10-5 в градуси или 1.6x10-7 в радиани. Тези числа са на границата на точност на 32 битово число с плаваща запетая (float), според мен е добре да се работи с 64 битово число (double).
--- Край на цитат ---
dmitarp, аз чета отгоре-отгоре, но ако може да поясните нещо за мен.
Защо се налага да изчислявате с абсолютната стойност на ъгъла? За даден ареал, най-голямото отклонение е последните 4-5 цифри, или за голяма точност 8 бита са достатъчни ::) - може и да бъркам.
Не съвсем точен пример, но:
43.418968, и.д. = 24.626770, и втора точка с.ш. = 43.419409, и.д.=24.626914.
Вижда се, че първите 4 цифри не се променят за целият ареал, защо е нужно да се "щракат" всеки път?
Навигация
[0] Списък на темите
Премини на пълна версия