From: Ivan Shmakov Document-Id: urn:uuid:ab96605b-83b3-4e59-b16a-7bf04d69ce62 License: CC0-1.0 Решалась задача отслеживания пространственного положения летательного аппарата в пространстве по данным, получаемым с контроллера SpeedyBee F405 v3 [1] с прошивкой ArduPilot [2, 3]. [1] http://speedybee.com/speedybee-f405-v3-bls-50a-30x30-fc-esc-stack/ [2] http://github.com/ArduPilot/ardupilot [3] http://firmware.ardupilot.org/Copter/stable-4.5.7/speedyb%65ef4v3/features.txt Программа mav1trak реализует следующий алгоритм: 1. командой MAV_CMD_SET_MESSAGE_INTERVAL (пакет COMMAND_INT) запрашивается получение пакетов телеметрии SCALED_IMU (данные инерциальных датчиков) и ATTITUDE (определяемая прошивкой ориентация аппарата в пространстве); 2. данные собираются в пары (ускорение, ориентация) согласно включенным в пакеты телеметрии временным отметкам; 3. получаемые данные ускорения в системе координат контроллера приводятся к системе координат «север, восток, вниз» (NED; направления условны, ввиду отсутствия магнитометра); 4. первые N полученных после начала работы (сброса) алгоритма NED-ускорений усредняются и принимаются за вектор ускорения свободного падения; 5. из следующих получаемых NED-ускорений вычитается найденный вектор ускорения свободного падения; 6. методом Эйлера решается система обыкновенных дифференциальных уравнений: dx _i = v _i dt, dv _i = a _i dt, i = 1, .., 3; шаг по времени — разность между временными отметками для текущей и предыдущей пары (ускорение, ориентация); 7. выводятся NED-вектора положения x, скорости v и ускорения (за вычетом ускорения свободного падения.) Пример вывода (контроллер неподвижен; время t в мс с отсечением полных секунд; положение x _i в мм относительно начальной точки; скорость v _i в мм с ^{-1}; ускорение a _i в мм с ^{-2}): t x_1 v_1 a_1 x_2 v_2 a_2 x_3 v_3 a_3 813.0 N 0 -6 -6 E 0 3 3 D 0 0 0 838.0 N -6 9 15 E 3 5 2 D 0 -12 -12 860.0 N 3 13 4 E 9 5 -0 D -12 -3 9 883.0 N 16 7 -5 E 14 -13 -17 D -15 10 11 905.0 N 24 12 4 E 0 -13 0 D -5 19 9 928.0 N 37 10 -2 E -14 -23 -9 D 17 20 1 953.0 N 49 3 -6 E -39 -19 3 D 39 9 -10 975.0 N 52 0 -2 E -61 -12 6 D 50 31 19 998.0 N 52 -0 -0 E -76 -23 -9 D 86 32 0 020.5 N 52 -6 -5 E -103 -18 4 D 124 32 -0 043.0 N 45 0 5 E -125 -17 0 D 162 43 9 068.0 N 46 5 4 E -146 -18 -1 D 215 42 -1 090.0 N 52 11 5 E -169 -17 0 D 268 53 9 113.0 N 66 1 -8 E -192 -22 -4 D 336 67 11 135.0 N 67 6 4 E -221 -23 -0 D 424 66 -1 158.0 N 75 17 9 E -251 -11 8 D 512 63 -2 183.0 N 98 28 8 E -266 -1 8 D 598 48 -12 205.5 N 137 55 20 E -267 7 5 D 664 71 17 228.0 N 213 68 10 E -258 20 9 D 763 82 8 250.0 N 310 74 4 E -230 20 0 D 880 80 -1 273.0 N 418 75 0 E -201 8 -8 D 996 81 0 298.0 N 528 87 8 E -189 21 9 D 1115 63 -12 320.0 N 658 82 -3 E -158 29 5 D 1210 63 -0 344.0 N 782 87 4 E -114 29 -0 D 1305 46 -11 365.0 N 917 101 9 E -69 43 9 D 1376 43 -2 388.0 N 1076 107 3 E -3 43 -0 D 1444 27 -11 413.0 N 1246 130 14 E 65 37 -3 D 1486 39 8 435.0 N 1455 130 -0 E 125 59 14 D 1550 37 -1 458.0 N 1667 123 -4 E 221 67 5 D 1611 37 -0 480.0 N 1870 137 9 E 333 82 9 D 1672 33 -2 503.0 N 2101 172 21 E 471 66 -9 D 1728 13 -12 528.0 N 2394 181 5 E 584 67 0 D 1750 12 -1 550.0 N 2706 175 -3 E 699 76 5 D 1771 11 -0 573.0 N 3012 191 9 E 831 91 9 D 1790 -10 -12 595.0 N 3351 201 5 E 992 92 1 D 1773 -11 -1 618.0 N 3711 210 5 E 1156 93 1 D 1753 -13 -1 Как видно, погрешность определения ускорения в одной из итераций достигает 27 мм с ^{-2}; накопленная ошибка положения — более 4 м за 0.8 с. Вполне вероятно, что матрица поворота в текущей редакции кода применена неверно (это следует проверить отдельно.) За основу взят рисунок [4] и раздел [5] статьи Википедии, однако ориентация осей акселерометра, похоже, отлична от принятой в аэронавтике. (Как соотносятся даваемые контроллером углы крена, тангажа и рыскания с направлением этих осей тоже не вполне ясно.) [4] http://commons.wikimedia.org/wiki/File:Yaw_Axis_Corrected.svg [5] http://en.wikipedia.org/wiki/Euler_angles#Conventions Обратимся поэтому к исходным пакетам телеметрии, полученным с контроллера для вывода выше (временным отметкам 838.0‒928.0 выше соответствуют значения t, мс 139838.0‒139928.0 ниже.) Info: 139813.0 gravity acquired, N 455.136 E 1921.35 D 9691.56 01:01 S-Atti 139838 -0.0765678 -0.115646 0.473328 -0.000585506 3.37344e-05 -0.000391611 01:01 S-IMU 139838 -115 76 -998 2 -14 0 0 0 0 3606 01:01 S-Atti 139860 -0.0765923 -0.115635 0.473353 -0.00121726 0.000636321 0.00111368 01:01 S-IMU 139860 -116 75 -1000 2 -13 2 0 0 0 3604 01:01 S-Atti 139883 -0.0766268 -0.115668 0.473395 -0.000787425 -0.000450701 0.00142786 01:01 S-IMU 139883 -116 73 -1000 2 -15 2 0 0 0 3604 01:01 S-Atti 139905 -0.0766517 -0.115661 0.473423 -0.000211315 2.20789e-05 0.00165376 01:01 S-IMU 139905 -116 75 -1000 3 -14 2 0 0 0 3604 01:01 S-Atti 139928 -0.0766076 -0.115667 0.47343 0.00167542 0.000514541 -1.33825e-05 01:01 S-IMU 139928 -116 74 -999 4 -14 1 0 0 0 3607 01:01 S-Atti 139953 -0.0766475 -0.115722 0.473452 -0.00147784 -0.00200757 0.000856987 Или же, сводя в таблицу и переводя ускорение в мм с ^{-2} и углы в градусы: t, мс |a| a _x a _y a _z крен тангаж рыскание 139838 9879.95 -1127.76 745.31 -9787.04 -4.39 -6.63 27.12 139860 9899.77 -1137.57 735.50 -9806.65 -4.39 -6.63 27.12 139883 9898.33 -1137.57 715.89 -9806.65 -4.39 -6.63 27.12 139905 9899.77 -1137.57 735.50 -9806.65 -4.39 -6.63 27.13 139928 9889.33 -1137.57 725.69 -9796.84 -4.39 -6.63 27.13 Повторюсь: контроллер — неподвижен (хотя и не строго горизонтален, так что углы выше, вероятно, близки к истине), поэтому наблюдаемый разброс в модуле вектора ускорения — почти 20 мм с ^{-2} — объясним лишь шумом датчика. Что можно предпринять? Во-первых, уточнить выполняемое преобразование координат: в ходе испытаний обнаружены по меньшей мере две ошибки. Во-вторых, вместо ATTITUDE можно запросить ATTITUDE_QUATERNION — поворот системы координат в таком случае можно выполнить точнее. Соответствующий математический аппарат автору, увы, незнаком. В-третьих, можно адаптировать код примеров [6]. В этом случае, однако, было бы желательно получить консультацию специалиста по теории оценивания [7]. [6] http://web.archive.org/web/20160426125212/http://www.x-io.co.uk/res/doc/madgwick_internal_report.pdf [7] http://ru.wikipedia.org/wiki/?curid=6250642 Наконец, можно увеличить частоту дискретизации (сейчас — 43.5 Гц) и (или) запросить пакет RAW_IMU вместо SCALED_IMU. Отметим, что попытка запросить пакет HIGHRES_IMU (105) дает следующий STATUSTEXT, свидетельствующий, видимо, об отсутствии поддержки данного пакета используемой прошивкой: 01:01 b3 217095.902292945 S-Text 6 0 0 :No ap_message for mavlink id (105) Увеличение частоты дискретизации позволит применить усреднение или медианный фильтр для подавления шума, однако это едва ли повысит точность на нужные три порядка (хотелось бы накапливать ошибку положения в 4 м за 800 с, не за 0.8 с.) Пакет RAW_IMU содержит более точную временную отметку (в мкс вместо мс для SCALED_IMU), в остальном же — эти пакеты несут идентичный набор данных; например: 01:01 S-IMU 2501319 -132 70 -996 0 0 0 0 0 0 4295 01:01 R-IMU 2501319402 -132 70 -996 0 0 0 0 0 0 0 4295 Поскольку в реализованном алгоритме используется еще и пакет ATTITUDE — с меньшим, чем для RAW_IMU, разрешением по времени — замена SCALED_IMU на RAW_IMU не представляется целесообразной. Использование кода [6], однако, сделает ATTITUDE ненужным, а значит использование RAW_IMU в этом случае может быть оправданным.