Динамический список препятствий на основе капсул
dynamicCapsuleList объект управляет двумя списками объектов столкновения на основе капсулы в 2-D пространстве. Объекты столкновения разделяются на два списка, эго-тела и препятствия. Для эго тела и препятствия в 3-D, см. dynamicCapsuleList3D объект.
Каждый объект столкновения в двух списках имеет три ключевых элемента:
ID -- целое число, определяющее каждый объект, сохраненный в EgoIDs свойство для эго-тел и ObstacleIDs свойство для препятствий.
Состояния - расположение и ориентация объекта в виде матрицы M-by-3, где каждая строка имеет форму [x y theta] и M - количество состояний вдоль пути объекта в мировой рамке. Список состояний предполагает, что каждое состояние разделено фиксированным временным интервалом. xy-позиции находятся в метрах, и theta находится в радианах. Локальное начало координат по умолчанию расположено в центре левого полукруга капсулы.
Геометрия - размер объекта на основе капсулы на основе заданной длины и радиуса. Радиус применяется к концевым крышкам полукруга, а длина - к длине центрального прямоугольника. Чтобы переместить геометрию капсулы и локальное начало координат относительно исходной точки по умолчанию, задайте фиксированное преобразование относительно локального кадра капсулы.
Используйте функции объекта для динамического добавления, удаления и обновления геометрии и состояний различных объектов в среде. Чтобы добавить эго-тело, см. addEgo объектная функция. Чтобы добавить препятствие, см. раздел addObstacle объектная функция.
После указания всех состояний объекта проверьте пути эго-тела и проверьте наличие столкновений с препятствиями на каждом шаге с помощью checkCollision объектная функция. Функция проверяет только то, сталкивается ли эго-тело с препятствием, игнорируя столкновения только между препятствиями или только эго-телами.
obstacleList = dynamicCapsuleList создает динамический капсульный список препятствий без эго-тел или препятствий. Чтобы начать построение списка препятствий, используйте addEgo или addObstacle функции объекта.
addEgo | Добавление эго-тел в список капсул |
addObstacle | Добавить препятствия 2-D список капсул |
checkCollision | Проверка на наличие столкновений между эго-телами и препятствиями |
egoGeometry | Геометрические свойства эго-тел |
egoPose | Позы эго-тел |
obstacleGeometry | Геометрические свойства препятствий |
obstaclePose | Позы препятствий |
removeEgo | Удалить эго-тела из списка капсул |
removeObstacle | Устранить препятствия из капсульного списка |
show | Отображение эго-тел и препятствий в окружающей среде |
updateEgoGeometry | Обновление геометрических свойств эго-тел |
updateEgoPose | Обновление состояния эго-тел |
updateObstacleGeometry | Обновление геометрических свойств препятствий |
updateObstaclePose | Обновление состояния препятствий |
Построить путь к эго-телу и поддерживать состояния препятствий с помощью dynamicCapsuleList объект. Визуализация состояний всех объектов среды в различных временных метках. Проверьте путь тела эго, проверив наличие столкновений с препятствиями в среде.
Создать dynamicCapsuleList объект. Извлеките максимальное количество шагов для использования в качестве временных отметок для путей объектов.
obsList = dynamicCapsuleList; numSteps = obsList.MaxNumSteps;
Добавить Ego Body
Определите эго-тело, указав идентификатор, геометрию и состояние вместе в структуре. Геометрия капсулы имеет длину 3 м и радиус 1 м. Укажите состояние как линейный путь от x = 0m до x = 100m.
egoID1 = 1; geom = struct("Length",3,"Radius",1,"FixedTransform",eye(3)); states = linspace(0,1,obsList.MaxNumSteps)'.*[100 0 0]; egoCapsule1 = struct('ID',egoID1,'States',states,'Geometry',geom); addEgo(obsList,egoCapsule1); show(obsList,"TimeStep",[1:numSteps]); ylim([-20 20])
Добавить препятствия
Укажите состояния для двух препятствий, которые отделены от тела эго на 5 м в противоположных направлениях по оси y. Предположим, что препятствия имеют одинаковую геометрию geom как тело эго.
obsState1 = states + [0 5 0]; obsState2 = states + [0 -5 0]; obsCapsule1 = struct('ID',1,'States',obsState1,'Geometry',geom); obsCapsule2 = struct('ID',2,'States',obsState2,'Geometry',geom); addObstacle(obsList,obsCapsule1); addObstacle(obsList,obsCapsule2); show(obsList,"TimeStep",[1:numSteps]); ylim([-20 20])
Обновить препятствия
Изменение местоположения препятствий и размеров геометрии с течением времени. Используйте ранее созданную структуру, измените поля и обновите препятствия, используя updateObstacleGeometry и updateObstaclePose функции объекта. Уменьшает радиус первого препятствия до 0,5 м и изменяет траекторию, чтобы переместить его в сторону тела эго.
obsCapsule1.Geometry.Radius = 0.5; obsCapsule1.States = ... [linspace(0,100,numSteps)' ... % x linspace(5,-4,numSteps)' ... % y zeros(numSteps,1)]; % theta updateObstacleGeometry(obsList,1,obsCapsule1); updateObstaclePose(obsList,1,obsCapsule1);
Проверка наличия коллизий
Визуализация новых путей. Показать, где происходит столкновение тела эго с препятствием, которое выделяется красным цветом. Обратите внимание, что столкновения между препятствиями не проверяются.
show(obsList,"TimeStep",[1:numSteps],"ShowCollisions",1); ylim([-20 20]) xlabel("X (m)") ylabel("Y (m)")
Программно проверять наличие конфликтов с помощью checkCollision объектная функция. Функция возвращает вектор логических значений, который указывает состояние каждого временного шага. Вектор транспонируется в целях отображения.
collisions = checkCollision(obsList)'
collisions = 1x31 logical array
0 0 0 0 0 0 0 0 0 0 0 0 1 1 1 1 1 1 1 1 1 1 0 0 0 0 0 0 0 0 0
Чтобы проверить пути с большим количеством шагов, используйте any функция на векторе значений коллизий.
if any(collisions) disp("Collision detected.") end
Collision detected.
Обновить путь Ego
Укажите новый путь для тела эго. Снова визуализируйте контуры, отображая конфликты.
egoCapsule1.States = ... [linspace(0,100,numSteps)' ... % x 3*sin(linspace(0,2*pi,numSteps))' ... % y zeros(numSteps,1)]; % theta updateEgoPose(obsList,1,egoCapsule1); show(obsList,"TimeStep",[1:numSteps],"ShowCollisions",1); ylim([-20 20])
