Динамический список препятствий на основе капсул
dynamicCapsuleList3D объект управляет двумя списками объектов столкновения на основе капсулы в 3-D пространстве. Объекты столкновения разделяются на два списка, эго-тела и препятствия. Для эго тела и препятствия в 2-D, см. dynamicCapsuleList объект.
Каждый объект столкновения в двух списках имеет три ключевых элемента:
ID -- целое число, определяющее каждый объект, сохраненный в EgoIDs свойство для эго-тел и ObstacleIDs свойство для препятствий.
Состояния - расположение и ориентация объекта в виде матрицы M-by-6, где каждая строка имеет форму [x y z qW qX qY qZ], и M - количество состояний вдоль пути объекта в мировой рамке. Список состояний предполагает, что каждое состояние разделено фиксированным временным интервалом. xyz-положения находятся в метрах, а ориентация является четырехэлементным кватернионным вектором. Локальное начало координат по умолчанию расположено в центре левого полушария капсулы.
Геометрия - размер объекта на основе капсулы в зависимости от заданной длины и радиуса. Радиус применяется к сферическим концам, а длина - к длине цилиндра. При сдвиге геометрии капсулы и локального начала относительно исходной точки по умолчанию задайте преобразование 4 на 4 относительно локального кадра капсулы. Чтобы сохранить преобразование по умолчанию, укажите eye(4).
Используйте функции объекта для динамического добавления, удаления и обновления геометрии и состояний различных объектов в среде. Чтобы добавить эго-тело, см. addEgo объектная функция. Чтобы добавить препятствие, см. раздел addObstacle объектная функция.
После указания всех путей объектов проверьте пути эго-тела и проверьте наличие столкновений с препятствиями на каждом шаге с помощью checkCollision объектная функция. Функция проверяет только то, сталкивается ли эго-тело с препятствием, игнорируя столкновения только между препятствиями или только эго-телами.
obstacleList = dynamicCapsuleList3D создает динамический капсульный список препятствий без эго-тел или препятствий. Чтобы начать построение списка препятствий, используйте addEgo или addObstacle функции объекта.
addEgo | Добавление эго-тел 3D список капсул |
addObstacle | Добавить препятствия 3-D список капсул |
checkCollision | Проверка на наличие столкновений между эго-телами и препятствиями |
egoGeometry | Геометрические свойства эго-тел |
egoPose | Позы эго-тел |
obstacleGeometry | Геометрические свойства препятствий |
obstaclePose | Позы препятствий |
removeEgo | Удалить эго-тела из списка капсул |
removeObstacle | Устранить препятствия из капсульного списка |
show | Отображение эго-тел и препятствий в окружающей среде |
updateEgoGeometry | Обновление геометрических свойств эго-тел |
updateEgoPose | Обновление состояния эго-тел |
updateObstacleGeometry | Обновление геометрических свойств препятствий |
updateObstaclePose | Обновление состояния препятствий |
Построить путь к эго-телу и поддерживать состояния препятствий с помощью dynamicCapsuleList3D объект. Визуализация состояний всех объектов среды в различных временных метках. Проверьте путь тела эго, проверив наличие столкновений с препятствиями в среде.
Создать dynamicCapsuleList3D объект. Извлеките максимальное количество шагов для использования в качестве временных отметок для путей объектов.
obsList = dynamicCapsuleList3D; numSteps = obsList.MaxNumSteps;
Добавить Ego Body
Определите эго-тело, указав идентификатор, геометрию и состояние вместе в структуре. Геометрия капсулы имеет длину 3 м и радиус 1 м. Укажите состояние как линейный путь от x = 0 м до x = 100 м.
egoID1 = 1; geom = struct("Length",3,"Radius",1,"FixedTransform",eye(4)); states = linspace(0,1,obsList.MaxNumSteps)'.*[100 0 0]; states = [states ones(numSteps,2) zeros(numSteps,2)]; egoCapsule1 = struct('ID',egoID1,'States',states,'Geometry',geom); addEgo(obsList,egoCapsule1); show(obsList,"TimeStep",1:numSteps); ylim([-20 20]) zlim([-5 20]) view(-45,25) hold on
Добавить препятствия
Укажите состояния для двух препятствий, отделенных от тела эго на 5 м в противоположных направлениях по оси Y. Предположим, что препятствия имеют одинаковую геометрию geom как тело эго.
obsState1 = states + [0 5 0 0 0 0 0]; obsState2 = states + [0 -5 0 0 0 0 0]; obsCapsule1 = struct('ID',1,'States',obsState1,'Geometry',geom); obsCapsule2 = struct('ID',2,'States',obsState2,'Geometry',geom); addObstacle(obsList,obsCapsule1); addObstacle(obsList,obsCapsule2); cla show(obsList,"TimeStep",1:numSteps);
Обновить препятствия
Изменение местоположения препятствий и размеров геометрии с течением времени. Используйте ранее созданную структуру, измените поля и обновите препятствия, используя updateObstacleGeometry и updateObstaclePose функции объекта. Уменьшите радиус первого препятствия до 0,5 м и измените траекторию, чтобы переместить его в сторону тела эго.
obsCapsule1.Geometry.Radius = 0.5; obsCapsule1.States = ... [linspace(0,100,numSteps)' ... % x linspace(5,-4,numSteps)' ... % y zeros(numSteps,1) ... % z ones(numSteps,2) zeros(numSteps,2)]; % quaternion % quaternion updateObstacleGeometry(obsList,1,obsCapsule1); updateObstaclePose(obsList,1,obsCapsule1);
Проверка наличия коллизий
Визуализация новых путей. Показать, где происходят столкновения между эго-телом и препятствием, которое выделяется красным цветом. Обратите внимание, что столкновения между препятствиями не проверяются.
cla show(obsList,"TimeStep",1:numSteps,"ShowCollisions",1);
Программно проверять наличие конфликтов с помощью 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)... % z ones(numSteps,2) zeros(numSteps,2)]; %quaternion % quaternion updateEgoPose(obsList,1,egoCapsule1); cla show(obsList,"TimeStep",1:numSteps,"ShowCollisions",1);
