Добавить препятствия 2-D список капсул
addObstacle( добавляет одно или несколько препятствий к списку динамических капсул 2-D с указанными значениями идентификатора, состояния и геометрии, указанными в capsuleListObj,obstacleStruct)obstacleStruct.
status = addObstacle(capsuleListObj,obstacleStruct)
Построить путь к эго-телу и поддерживать состояния препятствий с помощью 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])
capsuleListObj - Перечень динамических капсулdynamicCapsuleList объектДинамический список капсул, указанный как dynamicCapsuleList объект.
obstacleStruct - Параметры препятствияПараметры препятствия, заданные как структура N-элемента или массив структуры, где N - количество добавленных препятствий. Поля каждой структуры определяют идентификатор, геометрию и состояния препятствия:
ID - Целое число, идентифицирующее каждый объект. Хранится в ObstacleIDs имущества dynamicCapsuleList объект, указанный capsuleListObj аргумент.
States - Расположение и ориентация объекта как матрицы M-by-3, где каждая строка имеет вид [x y theta], и M - количество состояний для указанного препятствия в мировой рамке. Список состояний предполагает, что каждое состояние разделено фиксированным временным интервалом. xy-позиции в метрах и theta находится в радианах.
Geometry - Структура с полями Length, Radius, и FixedTransform. Эти поля определяют размер объекта на основе капсулы, используя заданную длину для цилиндра и радиус полукруга для концевых крышек. Чтобы сместить геометрию капсулы от исходной точки по умолчанию, укажите FixedTransform поле как фиксированное преобразование относительно локального кадра капсулы. Чтобы сохранить начало капсулы по умолчанию, укажите преобразование как eye(3).
