Список препятствий на основе динамических капсул
The dynamicCapsuleList объект управляет двумя списками объектов столкновения на основе капсул в 2-D пространстве. Объекты столкновения разделяются на два списка, тела эго и препятствия. Для ego тел и препятствий в 3-D, смотрите dynamicCapsuleList3D объект.
Каждый объект столкновения в двух списках имеет три ключевых элемента:
ID -- Целое число, которое идентифицирует каждый объект, сохраненный в EgoIDs свойство для ego тел и ObstacleIDs свойство для препятствий.
Состояния - Расположение и ориентация объекта как матрицы M-на-3, где каждая строка имеет форму [x y theta] и M количество состояний вдоль пути объекта в мировой системе координат. Список состояний предполагает, что каждое состояние разделено фиксированным временным интервалом. xy -положения указаны в метрах, и theta находится в радианах. Локальный источник по умолчанию расположен в центре левого полукруга капсулы.
Геометрия - Размер объекта на основе капсулы на основе заданных длины и радиуса. Радиус применяется к концевым прописным буквам полукругов, а длина - к длине центрального прямоугольника. Чтобы переместить геометрию капсулы и локальный источник относительно исходной точки по умолчанию, задайте фиксированное преобразование относительно локальной системы координат капсулы.
Используйте функции объекта для динамического добавления, удаления и обновления геометрий и состояний различных объектов в окружение. Чтобы добавить тело ego, смотрите addEgo функция объекта. Чтобы добавить препятствие, смотрите addObstacle функция объекта.
После определения всех состояний объекта проверяйте пути ego-body и проверяйте на столкновения с препятствиями на каждом шаге, используя checkCollision функция объекта. Функция проверяет только, сталкивается ли тело эго с препятствием, игнорируя столкновения между только препятствиями или только телами эго.
obstacleList = dynamicCapsuleList создает динамический список препятствий на основе капсул без эго-тел или препятствий. Чтобы начать создание списка препятствий, используйте addEgo или addObstacle функции объекта.
addEgo | Добавьте тела ego в список капсул |
addObstacle | Добавьте препятствия к 2-D капсулы списка |
checkCollision | Проверяйте на столкновения между эго-телами и препятствиями |
egoGeometry | Геометрические свойства эго тел |
egoPose | Положения эго-тел |
obstacleGeometry | Геометрические свойства препятствий |
obstaclePose | Положения препятствий |
removeEgo | Удалите тела эго из списка капсул |
removeObstacle | Удаление препятствий из списка капсул |
show | Отображение эго-тел и препятствий в окружении |
updateEgoGeometry | Обновление геометрических свойств эго-тел |
updateEgoPose | Обновление состояний эго-тел |
updateObstacleGeometry | Обновление геометрических свойств препятствий |
updateObstaclePose | Обновление состояний препятствий |
Создайте путь к эго-телу и сохраните состояния препятствий, используя dynamicCapsuleList объект. Визуализируйте состояния всех объектов в окружении в различных временных метках. Проверьте путь тела ego путем проверки на столкновения с препятствиями в окружении.
Создайте dynamicCapsuleList объект. Извлеките максимальное количество шагов для использования в качестве количества меток времени для путей к объекту.
obsList = dynamicCapsuleList; numSteps = obsList.MaxNumSteps;
Добавить тело Ego
Задайте тело ego путем определения идентификатора, геометрии и состояния вместе в структуре. Геометрия капсулы имеет длину 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
Задайте новый путь для тела 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])
