Запланируйте и выполните траектории без коллизий Используя манипулятор КИНОВОЙ 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¨. Движущими силами модели дают

x˙=[0I700]x+[0I7]u

где I7 обозначает 7×7 единичная матрица. Выход модели задан как

y=[I70]x.

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

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

J=0T(pref-p(q(t)))Qr(pref-p(q(t)))+u(t)Quu(t)dt+(pref-p(q(T)))Qt(pref-p(q(T)))+q˙(T)Qvq˙(T)

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

Матрицы Qr, Qu, Qt, и Qv постоянные матрицы веса.

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

di,jdsafe

Здесь, di,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.

Для просмотра документации необходимо авторизоваться на сайте