Планирование и выполнение траекторий без столкновения с помощью KINOVA Gen3 Manipulator

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

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

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

Загрузите модель KINOVA Gen3 древовидного твердого тела (RBT).

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 объекты как препятствия, которых нужно избегать. Измените следующее логическое значение, чтобы планировать использование статических, а не движущихся препятствий.

isMovingObst = true;

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

helperCreateObstaclesKINOVA;

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

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

Figure contains an axes. The axes 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$ - тело робота в $j$ -е препятствие, вычисленное с помощью 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) объект. Каждая итерация вычисляет положение, скорость и ускорение соединений, чтобы избежать препятствий, когда они движутся к цели. The helperCheckGoalReachedKINOVA скрипт проверяет, достиг ли робот цели. The 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.

Выполните запланированную траекторию с помощью симуляции и управления роботом Joint-Space

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

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

visTimeStep = 0.2;

Использование jointSpaceMotionModel отслеживать траекторию с вычисленным крутящим моментом. The 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. The axes 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] Schulman, J., et al. Планирование движения с последовательной выпуклой оптимизацией и проверкой выпуклого столкновения. Международный журнал исследований робототехники 33.9 (2014): 1251-1270.

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