Запланируйте и Выполните использование Траекторий Без коллизий Манипулятор КИНОВОЙ Gen3

Этот пример использует:

В этом примере показано, как запланировать траектории робота без коллизий с обратной связью от начальной буквы до желаемого положения исполнительного элемента конца с помощью нелинейного прогнозирующего управления модели. Получившиеся траектории выполняются с помощью имитационной модели робота низкого качества и вычислили управление крутящим моментом. Препятствия могут быть статическими или динамическими, и могут быть или установлены как примитивы (сферы, цилиндры, поля) или как пользовательские сетки.

Описание робота и положения

Загрузите модель дерева твердого тела (RBT) КИНОВОЙ Gen3.

robot = loadrobot('kinovaGen3', 'DataFormat', 'column');

Получите количество соединений.

numJoints = numel(homeConfiguration(robot));

Задайте систему координат робота, где исполнительный элемент конца присоединяется.

endEffector = "EndEffector_Link";

Задайте начальные и желаемые положения исполнительного элемента конца. Используйте инверсную кинематику, чтобы решить для начальной настройки робота, учитывая желаемое положение.

% Initial end-effector pose
taskInit = trvec2tform([[0.4 0 0.2]])*axang2tform([0 1 0 pi]);

% Compute current robot joint configuration using inverse kinematics
ik = inverseKinematics('RigidBodyTree', robot);
ik.SolverParameters.AllowRandomRestart = false;
weights = [1 1 1 1 1 1];
currentRobotJConfig = ik(endEffector, taskInit, weights, robot.homeConfiguration);
currentRobotJConfig = wrapToPi(currentRobotJConfig);

% Final (desired) end-effector pose
taskFinal = trvec2tform([0.35 0.55 0.35])*axang2tform([0 1 0 pi]);  
anglesFinal = rotm2eul(taskFinal(1:3,1:3),'XYZ');
poseFinal = [taskFinal(1:3,4);anglesFinal']; % 6x1 vector for final pose: [x, y, z, phi, theta, psi]

Сетки столкновения и препятствия

Инициализируйте функцию помощника, которая использует checkCollision проверять на столкновения между корпусами робота и миром столкновения. Установка ExhaustiveChecking свойство к true включает проверку столкновения все тела.

collisionHelper = helperManipCollsionsKINOVA(robot);
collisionHelper.ExhaustiveChecking = true;

Чтобы проверять на и избежать столкновений во время управления, необходимо установить столкновение world как набор объектов столкновения. Этот пример использует collisionSphere объекты как препятствия, чтобы избежать. Измените следующую булевскую переменную, чтобы запланировать использование, статическое вместо движущихся препятствий.

isMovingObst = true;

Размеры препятствия и местоположения инициализируются в следующей функции помощника. Чтобы добавить больше статических препятствий, добавьте объекты столкновения в world массив.

helperCreateObstaclesKINOVA;

Визуализируйте робота в начальной настройке. Необходимо видеть препятствия в среде также.

x0 = [currentRobotJConfig', zeros(1,numJoints)];
helperInitialVisualizerKINOVA;

Задайте расстояние безопасности далеко от препятствий. Это значение используется в функции ограничения неравенства нелинейного контроллера MPC.

safetyDistance = 0.01;

Спроектируйте нелинейный прогнозирующий контроллер модели

Можно спроектировать нелинейный прогнозирующий контроллер модели, использующий следующий файл помощника, который создает nlmpc объект контроллера. К представлениям файл введите edit helperDesignNLMPCobjKINOVA.

helperDesignNLMPCobjKINOVA;

Контроллер спроектирован на основе следующего анализа. Максимальный номер итераций для решателя оптимизации определяется к 5. Нижние и верхние границы для объединенного положения и скоростей (состояния) и ускорения (входные параметры управления) установлены явным образом.

Модель соединений робота описана двойными интеграторами. Состояния модели $x = [q, q_{}^{˙}]$ , где 7 объединенных положений обозначаются $q$ и их скорости обозначаются $q_{}^{˙}$ . Входные параметры модели являются объединенными ускорениями $u = q_{}^{¨}$ . Движущими силами модели дают

$\dot{x} = [\begin{matrix} 0 & I_{7} \\ 0 & 0 \end{matrix}] x + [\begin{matrix} 0 \\ I_{7} \end{matrix}] u$

где $I_{7}$ обозначает $7 \times 7$ единичная матрица. Выход модели задан как

$y = [\begin{matrix} I_{7} & 0 \end{matrix}] x$ .

Поэтому нелинейный прогнозирующий контроллер модели (nlobj) имеет 14 состояний, 7 выходных параметров и 7 входных параметров.

Функция стоимости для nlobj пользовательская нелинейная функция стоимости, которая задана способом, похожим на квадратичные издержки плюс отслеживания терминальная стоимость.

$J = \int_{0}^{T} {(p_{ref} - p (q (t)))}^{'} Q_{r} (p_{ref} - p (q (t))) + u^{'} (t) Q_{u} u (t) dt + {(p_{ref} - p (q (T)))}^{'} Q_{t} (p_{ref} - p (q (T))) + {\dot{q}}^{'} (T) Q_{v} \dot{q} (T)$

Здесь, $p (q (t))$ преобразовывает объединенные положения $q (t)$ к системе координат исполнительного элемента конца с помощью прямой кинематики и getTransform, и $p_{ref}$ обозначает желаемое положение исполнительного элемента конца.

Матрицы $Q_{r}$ , $Q_{u}$ , $Q_{t}$ , и $Q_{v}$ постоянные матрицы веса.

Чтобы избежать столкновений, контроллер должен удовлетворить следующим ограничениям неравенства.

$d_{i, j} \geq d_{safe}$

Здесь, $d_{i, j}$ обозначает расстояние от $i$ - корпус робота th к $j$ - препятствие th, вычисленное использование checkCollision.

В этом примере, $i$ принадлежит ${1, 2, 3, 4, 5, 6}$ (основа и корпуса робота исполнительного элемента конца исключены), и $j$ принадлежит ${1, 2}$ (2 препятствия используются).

Якобианы функции состояния, выходной функции, функции стоимости и ограничений неравенства все обеспечиваются для модели предсказания, чтобы повысить эффективность симуляции. Чтобы вычислить якобиан ограничения неравенства, используйте geometricJacobian функционируйте и якобиевское приближение в [1].

Планирование траектории с обратной связью

Симулируйте робота максимум для 50 шагов с правильными начальными условиями.

maxIters = 50;
u0 = zeros(1,numJoints);
mv = u0;
time = 0;
goalReached = false;

Инициализируйте массив данных для управления.

positions = zeros(numJoints,maxIters);
positions(:,1) = x0(1:numJoints)';

velocities = zeros(numJoints,maxIters);
velocities(:,1) = x0(numJoints+1:end)';

accelerations = zeros(numJoints,maxIters);
accelerations(:,1) = u0';

timestamp = zeros(1,maxIters);
timestamp(:,1) = time;

Используйте nlmpcmove функция для генерации траектории с обратной связью. Задайте опции генерации траектории с помощью nlmpcmoveopt объект. Каждая итерация вычисляет положение, скорость и ускорение соединений, чтобы избежать препятствий, когда они двигают цель. helperCheckGoalReachedKINOVA скрипт проверяет, достиг ли робот цели. helperUpdateMovingObstacles скрипт перемещает местоположения препятствия на основе временного шага.

options = nlmpcmoveopt;
for timestep=1:maxIters
    disp(['Calculating control at timestep ', num2str(timestep)]);
    % Optimize next trajectory point 
    [mv,options,info] = nlmpcmove(nlobj,x0,mv,[],[], options);
    if info.ExitFlag < 0
        disp('Failed to compute a feasible trajectory. Aborting...')
        break;
    end
    % Update states and time for next iteration
    x0 = info.Xopt(2,:);
    time = time + nlobj.Ts;
    % Store trajectory points
    positions(:,timestep+1) = x0(1:numJoints)';
    velocities(:,timestep+1) = x0(numJoints+1:end)';
    accelerations(:,timestep+1) = info.MVopt(2,:)';
    timestamp(timestep+1) = time;
    % Check if goal is achieved 
    helperCheckGoalReachedKINOVA;
    if goalReached
        break;
    end
    % Update obstacle pose if it is moving
    if isMovingObst
        helperUpdateMovingObstaclesKINOVA;
    end
end

Calculating control at timestep 1
Slack variable unused or zero-weighted in your custom cost function. All constraints will be hard.
Calculating control at timestep 2
Slack variable unused or zero-weighted in your custom cost function. All constraints will be hard.
Calculating control at timestep 3
Slack variable unused or zero-weighted in your custom cost function. All constraints will be hard.
Calculating control at timestep 4
Slack variable unused or zero-weighted in your custom cost function. All constraints will be hard.
Calculating control at timestep 5
Slack variable unused or zero-weighted in your custom cost function. All constraints will be hard.
Calculating control at timestep 6
Slack variable unused or zero-weighted in your custom cost function. All constraints will be hard.
Calculating control at timestep 7
Slack variable unused or zero-weighted in your custom cost function. All constraints will be hard.
Target configuration reached.

Выполните Запланированную Траекторию с помощью Управления Роботом Низкого качества и Симуляции

Обрежьте векторы траектории на основе временных шагов, вычисленных из плана.

tFinal = timestep+1;
positions = positions(:,1:tFinal);
velocities = velocities(:,1:tFinal);
accelerations = accelerations(:,1:tFinal);
timestamp = timestamp(:,1:tFinal);

visTimeStep = 0.2;

Используйте jointSpaceMotionModel отслеживать траекторию с управлением вычисленного крутящего момента. helperTimeBasedStateInputsKINOVA функция генерирует производные входные параметры для ode15s функция для моделирования вычисленной траектории робота.

motionModel = jointSpaceMotionModel('RigidBodyTree', robot);

% Control robot to target trajectory points in simulation using low-fidelity model
initState = [positions(:,1);velocities(:,1)];
targetStates = [positions;velocities;accelerations]';    
[t,robotStates] = ode15s(@(t,state) helperTimeBasedStateInputsKINOVA(motionModel, timestamp, targetStates, t, state), [timestamp(1):visTimeStep:timestamp(end)], initState);

Визуализируйте движение робота.

helperFinalVisualizerKINOVA;

[1] Шульман, J., и др. "Движение, планирующее с последовательной выпуклой оптимизацией и выпуклой проверкой столкновения". Международный журнал Исследования Робототехники 33.9 (2014): 1251-1270.

Документация