Запланируйте и выполните траектории без коллизий Используя манипулятор КИНОВОЙ 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);

% The IK solver respects joint limits, but for those joints with infinite
% range, they must be wrapped to a finite range on the interval [-pi, pi].
% Since the the other joints are already bounded within this range, it is
% sufficient to simply call wrapToPi on the entire robot configuration
% rather than only on the joints with infinite range.
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]

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

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

isMovingObst = true;

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

helperCreateObstaclesKINOVA;

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

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

Figure contains an axes object. The axes object contains 11 objects of type patch, line. These objects represent base_link, Shoulder_Link, HalfArm1_Link, HalfArm2_Link, ForeArm_Link, Wrist1_Link, Wrist2_Link, Bracelet_Link, EndEffector_Link, Shoulder_Link_mesh, HalfArm1_Link_mesh, HalfArm2_Link_mesh, ForeArm_Link_mesh, Wrist1_Link_mesh, Wrist2_Link_mesh, Bracelet_Link_mesh, base_link_mesh.

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

safetyDistance = 0.01;

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

Можно спроектировать нелинейный прогнозирующий контроллер модели, использующий следующий файл помощника, который создает nlmpc (Model Predictive Control Toolbox) объект контроллера. К представлениям файл введите 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 (Model Predictive Control Toolbox) функция для генерации траектории с обратной связью. Задайте опции генерации траектории с помощью nlmpcmoveopt Объект (Model Predictive Control Toolbox). Каждая итерация вычисляет положение, скорость и ускорение соединений, чтобы избежать препятствий, когда они двигают цель. 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;

Figure contains an axes object. The axes object contains 47 objects of type patch, line. These objects represent base_link, Shoulder_Link, HalfArm1_Link, HalfArm2_Link, ForeArm_Link, Wrist1_Link, Wrist2_Link, Bracelet_Link, EndEffector_Link, Shoulder_Link_mesh, HalfArm1_Link_mesh, HalfArm2_Link_mesh, ForeArm_Link_mesh, Wrist1_Link_mesh, Wrist2_Link_mesh, Bracelet_Link_mesh, base_link_mesh.

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

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