Ультразвуковая система определения координат 2.0

    Ультразвуковая система определения координат 2.0


    О чем эта статья: Эта статья описывает принципы работы моей системы определения координат, и мой опыт её изготовления. Данная статья не является инструкцией по изготовлению навигационной системы, ибо это не так просто, чтобы описать в одной статье.



    Данная статья является развитием идей моей предыдущей статьи:
    habr.com/ru/post/451408

    Структура системы


    Система определения координат имеет вот такую структурную схему:


    Рис. 1 — Принципиальная схема УЗ системы определения координат.

    Рассмотрим каждый элемент повнимательнее.

    УЗ приемник







    Рис. 2 — Развитие УЗ приемника (сверху вниз).

    Изначально приемники состояли из одного модуля и имели очень маленькую зону охвата помещения, в те времена я не оцифровывал сигнал, а просто получал с датчика длину, которую выдавал штатный контроллер.

    Затем для увеличения охвата я решил сделать модуль из трех датчиков, данные с которых параллельно оцифровывались и передавались на ПК, где рассчитывалось расстояние и координаты. После испытания трехкомпонентного приемника я увидел, что достаточно и одного приемника, если ты сам обрабатываешь сигнал.

    В результате приемник стал опять однокомпонентным, но из-за того что я сейчас сам обрабатываю сигнал, область покрытия стала достаточно большой. Сигнал сейчас обрабатывается в STM32 после оцифровки, на выход он выдает только расстояние.

    Состав:
    • STM32 – используется для оцифровки УЗ сигнала и расчета расстояния до излучателя;
    • HC-SR04 – я его немного модифицировал и теперь могу с его входа получать сигнал, представленный на рисунке 3;
    • RS485 – для передачи расстояния до излучателя на ПК.


    Рис. 3 — Оцифрованный УЗ сигнал.


    Рис. 4 — УЗ сигнал на участке 4700 – 5200 рисунка выше.

    УЗ излучатель







    Рис. 5 — Развитие излучателя (сверху вниз).

    Как вы поняли из рисунка 5, вначале я просто дергал лапкой Trig на датчике HC-SR04, это была не самая лучшая система, хотя бы потому, что я не мог сам определять сколько УЗ волн создать, не мог изменять их мощность, и этот излучатель был достаточно громоздким.

    Тогда я создал вторую версию, которая была намного более громоздкой, но я мог уже регулировать все при помощи STM32 и L293D, которые напрямую подключались к УЗ динамику. Теперь вместо штатных 5 вольт я подавал 17, и это сильно увеличило рабочую зону. Также я добавил радиомодуль и интерфейс RS485, что сделало этот модуль автономным.

    После ряда испытаний я решил уменьшить излучатель до приличных размеров и скомпоновал все поплотней, в результате получилось то, что вы видите на последней картинке.

    Состав:
    • STM32 – получает команду Trig (начать измерение расстояний) по радиоканалу и излучает УЗ сигнал, также передает команды, полученные из радиомодуля в RS485 (использую для дистанционного управления мобильной платформой);
    • RS485 – информационный интерфейс устройства для пользователя;
    • DC-DC повышающий – преобразует питающее 5В в 17В для излучателя;
    • L293D – использую для генерации сильного УЗ сигнала с напряжением 17В;
    • nRF24 – радиоканал;
    • Излучатели – 6 излучателей, которые выпаял из модуля HC-SR04.

    Базовый модуль



    Рис. 6 — Базовый модуль.

    Состав:
    • STM32 — посылает излучателю и приемникам Trig, получает команды по USB от ПК для передачи через радиоканал излучателю (он же мобильный модуль);
    • nRF24 – передача Trig излучателю и передача команд излучателю.

    ПК модуль


    ПК через RS485 переходник подключен ко всем УЗ приемникам, по данному интерфейсу получает от них все длины и высчитывает координаты мобильного модуля. Через USB подключен к базовому модулю и через него передает управляющие команды на мобильный модуль.

    Расположение модулей в рабочей зоне


    Карта комнаты с размещенными по углам УЗ приемниками выглядит вот так:


    Рис. 7 — Общая идея расположения датчиков.


    Рис. 8 — Вид сверху в масштабе (кружками, с цифрами внутри, обозначены датчики).

    Алгоритм работы УЗ системы определения координат


    1. Базовый модуль посылает команду Trig излучателю (по радиоканалу) и приемникам (по проводу).
    2. Излучатель начинает излучать, а приемники начинают слушать эфир.
    3. Каждый приемник, услышав УЗ сигнал, записывает время между командой Trig и временем получения этого сигнала и переводит это в расстояние.
    4. Приемники по RS485 отправляют расстояния от излучателя до себя на ПК.
    5. ПК рассчитывает координаты излучателя.

    Работа системы


    Запустив это все, мы получаем координаты объекта, точность зависит от места в комнате. В лучших местах погрешность не превышает пары сантиметров, а в худших… ну вы сами все можете увидеть на рисунке 9.


    Рис. 9 — Перемещение излучателя по комнате.

    На данном рисунке четыре набора координат, так как мы имеем четыре приемника, а для определения координат в трехмерном пространстве нам необходимо только три, то количество комбинаций приемников у нас четыре.

    Как видно из рисунка, в общем система определяет координаты верно, но в разных местах погрешность разная. Я перемещал излучатель параллельно осей Х и У, где-то с интервалом между линиями в метр, где-то в полметра, это видно на рисунке. Самая лучшая точность в центре, а по краям все плохо, ибо оттуда УЗ сигнал падает на приемник под большим углом, и это создает искажения в оцифрованном сигнале.

    После фильтрации и усреднения четырех пар координат мы получаем следующую картину:


    Рис. 10 — Усредненная траектория.

    Не айс, но уж что есть.

    Для интереса покажу, как это все выглядит в 3D, ведь третья координата у нас тоже есть, хотя она в моем проекте и не нужна, ведь мобильный робот перемещается только в плоскости.


    Рис. 11 — 3D траектория.

    Как мы видим, все точки лежат примерно в одной плоскости, и это корректно, ведь излучатель я возил по полу не отрывая.

    А вот пример неудачной проезда мобильной платформы с излучателем, траектория ее движения видна прекрасно. По замыслу платформа должна была вернуться в исходную точку, проехав по прямоугольной траектории, но не сложилось, в следующих статьях я расскажу почему.


    Рис. 12 — Траектория перемещения мобильной платформы.

    Актуальные проблемы


    У меня на данный момент есть целый ряд актуальных проблем, и если кто-то может что-то подсказать, прошу написать это в комментариях или в VK:
    vk.com/b__s__v

    1. Чем лучше принимать УЗ сигнал? Датчики HC-SR04, которые я сейчас использую для приема и усиления УЗ сигнала, не лучший вариант. Во-первых, потому что они всегда принимают сигнал только с одной частотой и по ним нельзя сделать частотное разделение источников сигнала (а мне это интересно попробовать), к тому же они слишком громоздкие, и я сильно от них зависим.
    2. Как убрать искажения координат в разных углах комнаты? На той сетке, которую я нарисовал на полу, видно что порой прямые начинают отклоняться, хотя я мобильный модуль всегда перемещал ровно, и от этого надо как-то избавляться.
    3. У nRF24 пропадает сигнал. Когда модули находятся близко, то проблем нет, но стоит их разнести на несколько метров и самому встать между ними, как сообщения доходят через одно. У меня там нет повторной отправки сообщений, так как я по этому каналу передаю синхросигналы, и они должны всегда приходить в ту же секунду. У меня модули с внешней антенной, и тот, что на базовом модуле лучше работает без антенны, чем с ней. Ничего не понимаю, мощность на максимуме.
    4. Места продвижения проекта. Если кто-то знает международные площадки типа Хабра на английском языке, скиньте ссылки, пожалуйста, а то я так ничего приличного и не нашел, а stackoverflow, это просто вопросы и ответы, не совсем то, чтобы выкладывать подобные статьи.
    5. Замечания и предложения. Если кто-то хочет высказаться, то буду рад и критике, и предложениям.

    Это далеко не финал моего проекта, хотя время и подходит к концу, но если будет интерес, то напишу статью о том, как на основе этой системы ездит мобильный робот по заданным координатам у меня в комнате.
    AdBlock похитил этот баннер, но баннеры не зубы — отрастут

    Подробнее
    Реклама

    Комментарии 14

      +1
      Ну как самое простое решение можно увеличить количество замеров. При построении построении геоподосновы обычно используют три группы по три замера. Считаем центры каждой группы и из этих центров считаем еще раз центр. Чтобы не увеличивать количество приемников можно покрутить передаичиками (в какой плоскости это отдельный вопрос)
        0
        да не факт что поможет, тут есть принципиальная проблема в том что всегда найдутся места где звук будет переотражаться и в итоге собрав переотражение 3 раза будем получать такую же кривую координату.
        0
        Автор, а вы про сонары слышали? Зачем пиарить ЭТО? Ну возьмите сделайте нормальное расположение излучателей/приемников, напишите алгоритмы не только определения дальности, но и азимута, и Допплера. Этих статей миллион найдете не только в области сонаров, но и радаров. Причем по стоимости изделие окажется примерно таким же, как ваше. Что касается позиционирования, то ищите статьи по radar SLAM. И будет у вас архитектурно выверенное решение, которое можно людям показать.
          +1
          Здравствуйте, насколько я знаю SLAM не способен определять координаты, как и сонары как и радары, к тому же ориентация по SLAM очень быстро портит карту до такой степени что определять позицию по ней даже примерно уже не возможно, радары и сонары больше для обнаружения препятствий. Я сейчас конечно же погуглил по этой теме, и ничего не нашел пригодного для определения положения мобильной платформы в помещении, если вам не трудно скиньте пожалуйста хоть одну ссылку.
            +1
            Вы наверно это семейство алгоритмов с чем-то путаете. SLAM — Simultaneous Localization and Mapping. Вы строите карту и рассчитываете свои координаты по определению этого алгоритма. Мне вот эта статья нравится www.uni-das.de/images/pdf/veroeffentlichungen/2017/01.pdf
            SLAM будет портить карту при неправильной калибровке сенсора. Если все более-менее нормально работает, углы/дальности разрешаются, то получите и карту, и навигацию.

            По поводу вашего метода триангуляции, если не хотите лезть во все эти дебри, попробуйте для начала простой фильтр Калмана к вашим измерениям прикрутить. С ним, скорей всего, будет лучше, чем с просто усреднением.
              +1
              Калманом кстати и состояние вашего робота можно будет определить, включая координаты и скорость.

              (А называться это будет Kalman-based SLAM)
                0
                Спасибо, почитаю.
          0
          если скорость передачи несильно важна, можно использовать LoRa/LoRaWAN.
            0

            SLAM вам правильно подсказали. И Kalman filter может подойти. Особенно, если у вас есть odometry хоть какая. Пусть даже команды поданные роботу — уже odometry. Wheel encoder конечно лучше будет, ну или хотя бы imu sensor. Короче читайте, изучайте, тут много математики, но все решается.

              0
              Одометрия есть, но там тоже все не идеально, у меня шаговые двигатели, это как энкодеры но лучше, так же есть BNO080 это насколько я знаю очень современный и хороший инерционный модуль, но я его пока использую только для определения угла поворота относительно вертикальной оси, я раньше использовал MPU6050, пытался с его помощью определить линейное перемещение, но у меня почему то очень быстро накапливались ошибки, и он был почти бесполезен, может у вас есть какой то опыт в этом, или ссылки на хорошие видео или примеры того как это сделать правильно? а то все при помощи таких модулей только кубиком крутят, а перемещение никто не пытается определять по нормальному
                0
                В том то и дело что только одним IMU не получится восстановить траекторию. Это как раз из-за постоянного дрифта у IMU сенсора. Гуглите IMU Dead Reconing. Шаговый двигатель это не тоже что я имею ввиду. Для одометрии энкодер нужен на пассивном колесе (или лучше двух), а не на моторе.
                Но, команды, поданные на шаговый двигатель — уже что то.

                Короче, как я понимаю у вас есть: (1) самодельный сенсор на ультразвуке, (2) не очень хорошая odometry из комманд к устройству/роботу, (3) 6-DoF IMU сенсор (теоретически).
                Этого уже вполне достаточно чтобы сделать fusion с помощью Kalman Filter, и уже тогда получить хорошую траекторию движения.
                Дрифт все-равно останется, но будет гораздо меньше.

                1-я проблема — Arduino это ни о чем. Нужен хотя бы Raspberry Pi, или что то подобное.
                2: Используйте ROS, там эти проблемы уже решены. Статьи по ROS есть даже на хабре, но лучше конечно читать в оригинале.
                3. Если у вас будут Raspberry Pi + ROS, то ультра-звуковые сенсоры уже можно использовать для построения карты проходимости вокруг робота.
                https://www.youtube.com/watch?v=eSeLW9Hkjhc
                Вот тут как раз энкодеры + лидар… Уже что-то можно сделать… Мда, наверно я погорячился с ультра-звуковыми :-))

                  0
                  Ультразвук здесь использован как маяки, а не как радар.
                  Можно использовать ToF датчики для построения карты проходимости, но в любом случае карта проходимости это уже следующий уровень для этой системы.
                    0
                    Спасибо, за комментарий, я в ближайшее время попробую IMU сенсор, оценю его реальную полезность в данном случае.
                    Скажите а как правильно это все подключать к Raspberry? датчик — STM32 — Raspberry, или напрямую? на каком языке логичнее писать? у меня больше опыта по МК и мало по ПК так что на каком языке лучше писать не особо понимаю, чтобы было легко и быстро
                      0
                      Вам нужно определиться какая конечная (или промежуточная) цель вашего проекта.
                      Если вы просто хотите изучать робототехнику, то все можно делать в ROS и в симуляторах — там можно достигнуть результатов гораздо быстрее.
                      В ROS вообще зачастую писать ничего не надо — просто конфигурировать существующие модули. Launch файлы и urdf (кинематика робота) написаны в XML.
                      Но при этом надо понимать что и как вы используете — т.е. нужны знания, и, соответственно, будете обучаться.
                      Код можно писать в Python, что особенно хорошо для новичков. Если не хватает производительности, можно всегда перейти на C/C++.

                      Теперь о вашем проекте. Если предположить, что у вас уже есть карта пространства (2D) то SLAM (Localiztion & Mapping) вам не нужен, а нужен только Localization.
                      В таком случае, можно использовать обычный Monte Carlo Localization / Particle Filter. И обычные ультра-звуковые сенсоры, смонтированные на роботе, (а не ваша супер-система) могут вам помочь гораздо больше.

                      Идея MCL в том, что если у вас 4 сенсора со всех сторон и они показывают какие-то значения, то на карте всего-лишь одна или несколько областей соответствует таким измерениям. (предпологается что на карте есть множественные стены/коридоры). MCL кидает множество рандомных точек с произвольной ориентацией (всевозможные позы робота) на карту и сверяет предсказанные измерения датчиков с существующими. Те позы, что дают минимальные ошибки запоминаются и используются на следующей итерации. Если робот двигается, и вы с помощью одометрии знаете как именно, то это значит что изменения в измерениях тоже можно предугадать. И если все правильно, то MCL сходтится к верному решению в течении нескольких фреймов/итераций.

                      По поводу вопроса о Raspberry: наверное стоит оставить Arduino как real-time контроллер, а на Raspberry крутить ROS, чтобы он локализацию считал, Калманы, одометрию и тому подобное.

              Только полноправные пользователи могут оставлять комментарии. Войдите, пожалуйста.

              Самое читаемое