В этом примере показано, как запланировать траектории робота без коллизий с обратной связью от начальной буквы до желаемого положения исполнительного элемента конца с помощью нелинейного прогнозирующего управления модели. Получившиеся траектории выполняются с помощью имитационной модели робота низкого качества и вычислили управление крутящим моментом. Препятствия могут быть статическими или динамическими, и могут быть или установлены как примитивы (сферы, цилиндры, поля) или как пользовательские сетки.
Загрузите модель дерева твердого тела (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. Нижние и верхние границы для объединенного положения и скоростей (состояния) и ускорения (входные параметры управления) установлены явным образом.
Модель соединений робота описана двойными интеграторами. Состояния модели , где 7 объединенных положений обозначаются и их скорости обозначаются . Входные параметры модели являются объединенными ускорениями . Движущими силами модели дают
где обозначает единичная матрица. Выход модели задан как
.
Поэтому нелинейный прогнозирующий контроллер модели (nlobj
) имеет 14 состояний, 7 выходных параметров и 7 входных параметров.
Функция стоимости для nlobj
пользовательская нелинейная функция стоимости, которая задана способом, похожим на квадратичные издержки плюс отслеживания терминальная стоимость.
Здесь, преобразовывает объединенные положения к системе координат исполнительного элемента конца с помощью прямой кинематики и getTransform
, и обозначает желаемое положение исполнительного элемента конца.
Матрицы , , , и постоянные матрицы веса.
Чтобы избежать столкновений, контроллер должен удовлетворить следующим ограничениям неравенства.
Здесь, обозначает расстояние от - корпус робота th к - препятствие th, вычисленное использование checkCollision
.
В этом примере, принадлежит (основа и корпуса робота исполнительного элемента конца исключены), и принадлежит (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.