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

В этом примере используются:

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

Описание робота и позы

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

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(Панель управления прогнозом модели) объект контроллера. Для просмотра файла введите edit helperDesignNLMPCobjKINOVA.

helperDesignNLMPCobjKINOVA;

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

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

$\overset{}{} \begin{matrix} _{} \end{matrix} \begin{matrix} _{} \end{matrix} x˙=[0I700]x+[0I7]u$

где $_{I7}$ обозначает единичную матрицу 7 × 7. Выходные данные модели определяются как

$y = \begin{matrix} _{[} \end{matrix} 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))) {\overset{'}{}}^{} Qt (_{} \overset{}{} pref-p$ (q (T)) +q˙ ′ (T) Qvq˙ (T)

Здесь $p (q (t$ )) преобразует $положения соединения q$ (t) в рамку концевого эффектора с использованием прямой кинематики иgetTransformи $_{pref}$ обозначает желаемую концевую эффекторную позу.

Матрицы $_{Qr}$ , $_{Q}$ , $_{Qt}$ и $_{Qv}$ являются матрицами постоянного веса.

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

$_{}_{di,j≥dsafe}$

Здесь $_{di,}$ 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(Панель инструментов управления с предсказанием модели). Каждая итерация вычисляет положение, скорость и ускорение соединений, чтобы избежать препятствий при их движении к цели. 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. 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., и др. «Планирование движения с последовательной оптимизацией выпуклостей и проверкой коллизий выпуклостей». The International Journal of Robotics Research 33.9 (2014): 1251 - 1270.

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