Проверить расстояние, создать силу
ROBOT CONTROL
Обход препятствий телом целиком
https://studywolf.wordpress.com/2016/11/24/full-body-obstacle-collision-avoidance/
Ранее уже обсуждалось, как избегать препятствий, используя DMP для траекторий наконечника-захвата робота. Тот подход хорош для управления одной отдельной точкой с массой, как перемещающийся в среде мобильный робот. Но если использовать эту систему для управления роботом-манипулятором, то быстро появится проблема: захват представляет собой не отдельную точку с массой, а он прикреплён к остальной части руки, и двигается таким образом, что звенья и суставы руки также должны обходить препятствия, что совсем другое дело.
Бегло осмотрев методы управления предотвращения столкновений сочленений (joint collision) с препятствиями, был немало удивлён, что в последнее время не так уж много разговоров было об этом. Тем не менее, в 1986 году была опубликована статья доктора Oussama Khatib, озаглавленная «Обход препятствий в режиме реального времени для манипуляторов и мобильных роботов» («Real-time obstacle avoidance for manipulators and mobile robots»), которая в значительной степени решает эту проблему, не прибегая к явным методам планирования путей. Может быть, поэтому с тех пор никаких статей об этом нет.
Тем не менее, я не смог найти в сети никаких реализаций данного подхода, а в нём есть несколько хитрых частей, поэтому в данной статье собираюсь разработать реализацию.
Примечание: в этой реализации препятствия представлены сферами. В целом всё работает довольно просто, можно сделать сферу достаточно большой, чтобы покрыть любое препятствие, которое нужно обойти. Если нужен более точный контроль, в приложении к своей статье доктор Khatib описывает математику для кубов и конусов. Для кубов вычисления ненамного сложнее, но для экономии места статьи, остановимся только на сферах.
|
|
|
Примечание: весь код вместе с VREP взаимодействием можно найти на моем github. Я предварительно рассчитал требуемые функции, потому что их генерация требует больших вычислительных ресурсов. Надеюсь, что сохранённые файлы не будут проблемой для разных ОС, но если станут, то попробуйте удалить все файлы в папке ur5_config и запустить код снова. Программа восстановит файлы (на моём ноутбуке это занимает ~4 часа, т.ч. будьте осторожны).
Общая идея
Алгоритм доктора Khatib работает путём определения в декартовых координатах сил для перемещения любой точки руки, находящейся слишком близко к препятствию, дальше от него. Этот алгоритм имеет вид:
Настройка
· Указать местоположение и размер препятствия
· Указать пороговое расстояние до препятствия
Время выполнения
· Найти ближайшую точку каждого звена руки к препятствиям
|
|
|
· Если препятствие в пределах порога, сгенерировать силу для применения к этой точке
· Преобразовать эту силу в крутящие моменты сочленений
· Добавить непосредственно к исходящему управляющему сигналу
Данная процедура похожа на добавление контроллера ноль-пространства, известный ранее в управлении в операционном пространстве. Здесь есть чёткое различие в том, что управляющий сигнал для обхода препятствий добавляется непосредственно к исходящему управляющему сигналу, т.е. он не фильтруется (как сигнал управления в ноль-пространстве), т.ч. нет гарантий, что это не повлияет на движение захвата. Фактически, влияние на движение захвата вполне желательно, поскольку важно также, как и добраться до цели, не протолкнуть руку через препятствие.
Настройка
Было упомянуто, что все препятствия будут рассматриваться как сферы. Алгоритм предполагает, что у есть список центров и радиусов препятствий.
Хочется, чтобы реакция уклонения системы была функцией расстояния до препятствия, чтобы чем ближе рука была к препятствию, тем сильнее реакция. Функция, которую описывает доктор Khatib, имеет следующую форму:
где 
– расстояние до цели, 
– пороговое расстояние до цели, в котором активируется функция уклонения, 
– это частная производная расстояния до цели по отношению к заданной точке на руке, 
и представляет собой коэффициент усиления.
|
|
|
Эта функция выглядит сложной, но на самом деле она довольно интуитивно понятна. Член частной производной в функции нужен для того, чтобы указывать в противоположном направлении от препятствия в декартовых координатах. Остальная часть выражения – это усиление, которое начинается с нуля, когда 
становится огромным, когда препятствие приближается к объекту (как 
). Использование 
и 
даёт функцию, которая выглядит так:
Итак, можно видеть, что скоро, сильный толчок от этого препятствия будет генерироваться, как только введём пороговое расстояние.
Но как узнать точное значение для порогового расстояния?
Найти ближайшую точку
Задачу обхода препятствий всем телом робота, оказывается, можно свести к задаче слежения только за ближайшей к препятствию точкой каждого звена руки, и отодвигать эту точку от препятствия, если было достигнуто пороговое расстояние.
Чтобы найти ближайшую точку к препятствию для данного звена руки, сделаем довольно аккуратное срабатывание. Посмотрите на код, затем обсудим это ниже.
|
|
|
В этом фрагменте p1 и p2 – начальное и конечное ( x , y , z ) положения звена руки (предполагаем, что движемся по прямой линии), а v – центр препятствия.
Сначала найдём линию направления звена руки, а затем линию из центра препятствия до начальной точки звена руки. Как только получим их, вычислим:
Используя геометрическое определение скалярного произведения (dot product) 2х векторов, можно переписать приведённое выше так:
это читается, как «величина vec_ob_line, делённая на величину vec_line, умноженная на угол между 2мя векторами».
· Если угол между векторами <0 (проекция projection также будет <0), то сразу же известно, что начало звена руки p1, является ближайшей точкой.
· Если значение проекции >1, то либо
o расстояние от начала звена руки до препятствия больше, чем длина руки,
o угол таков, что конец звена руки р2 есть ближайшая точка к препятствию.
· В последнем случае, ближайшая точка находится где-то вдоль звена руки.
Чтобы найти, где именно, делаем следующее:
что можно переписать
и переставить во втором слагаемом множители
Итак, всё, что делает это выражение – отодвигает нас от начала звена руки p1, а также определяет подвыражение: одна его часть указывает направление на vec_line, нормализованное до 1, а вторая его часть –точное расстояние вдоль vec_line до точки, в которой с препятствием сформируется прямой угол (задавая самое короткое расстояние).
Удобный рисунок из Википедии помогает наглядно проиллюстрировать подвыражение, где B – это vec_line, а A – vec_ob_line:
Вооружившись этой информацией, понимаем, как найти ближайшую точку к препятствию для каждого звена руки, и теперь готовы создать силу перемещения руки в противоположном направлении.
Проверить расстояние, создать силу
Чтобы вычислить расстояние, всё, что нужно, это вычислить евклидово расстояние от ближайшей точки звена руки до центра сферы, а затем вычесть радиус сферы:
Теперь можно проверить и сгенерировать 
, используя уравнение выше. Неспецифицированной частью этого уравнения была – частная производная расстояния до цели по отношению к ближайшей точке, её можно рассчитать как нормированную разность между 2мя точками:
Хорошо! Самая близкая точка найдена, и известна сила, которую нужно применить, – дальше обычная процедура в операционном пространстве.
Дата добавления: 2019-11-16; просмотров: 65; Мы поможем в написании вашей работы! |
Мы поможем в написании ваших работ!