Изменения для создания кода

Чтобы включить генерацию кода, адаптируйте код в окне Планирование и выполнение траекторий без коллизий с помощью KINOVA Gen3 Manapulator (Robotics System Toolbox), следуя приведенным ниже шагам.

- Пользовательская функция затрат: nlmpcCostFunctionKINOVACodeGen.m

- пользовательские ограничения неравенства: nlmpcIneqConFunctionKINOVACodeGen.m

- Функция состояния: nlmpcModelKINOVACodeGen.m

- Функция вывода: nlmpcOutputKINOVACodeGen.m

- Якобиан пользовательской функции затрат: nlmpcJacobianCostKINOVACodeGen.m

- Якобян о пользовательских ограничениях неравенства: nlmpcJacobianConstraintKINOVACodeGen.m

- Якобян государственной функции: nlmpcJacobianModelKINOVACodeGen.m

- Якобиан выходной функции: nlmpcJacobianOutputKINOVACodeGen.m

persistent robot world    
if isempty(robot)
    robot = loadrobot('kinovaGen3', 'DataFormat', 'column');  
    [world, ~] = helperCreateObstaclesKINOVA(posesNow);
end 

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

Загрузите модель дерева жесткого тела (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);
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 (Robotics System Toolbox) объекты как препятствия для предотвращения. Для планирования с использованием статических, а не движущихся объектов задайте isMovingObst кому false.

isMovingObst = true;

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

if isMovingObst == true
    helperCreateMovingObstaclesKINOVACodeGen
else
    posesNow = [0.4 0.4 0.25 ; 0.3 0.3 0.4];
end
[world, numObstacles] = helperCreateObstaclesKINOVACodeGen(posesNow);

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

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

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

safetyDistance = 0.01; 

Расчетный контроллер прогнозирования нелинейной модели

Можно спроектировать контроллер прогнозирования нелинейной модели с помощью helperDesignNLMPCobjKINOVACodeGen вспомогательный файл, который создает nlmpc объект контроллера. Для просмотра файла в командной строке MATLAB введите edit helperDesignNLMPCobjKINOVACodeGen.

helperDesignNLMPCobjKINOVACodeGen;

Планирование траектории с замкнутым контуром

Смоделировать робота максимум на 50 шагов с правильными исходными условиями.

Инициализация параметров моделирования

maxIters = 50;
u0 = zeros(1,numJoints);
mv = u0';
time = 0;
goalReached = false;

Создать код для объекта nlmpc

useMex = true;
buildMex = true;
if useMex
    [coredata, onlinedata] = getCodeGenerationData(nlobj,x0',u0',paras);
    if buildMex
        mexfcn = buildMEX(nlobj,'kinovaMex',coredata,onlinedata);
    end
end

Generating MEX function "kinovaMex" from nonlinear MPC to speed up simulation.
Code generation successful.

MEX function "kinovaMex" successfully generated.

Инициализация массива данных для управления

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;

Создать траекторию

Использовать созданный файл MEX kinovaMex вместо nlmpcmove функция ускорения формирования траектории замкнутого контура. Укажите опции создания траектории в диалоговом режиме с помощью аргумента onlineData.

Каждая итерация вычисляет положение, скорость и ускорение соединений, чтобы избежать препятствий при их движении к цели. helperCheckGoalReachedKINOVACodeGen сценарий проверяет, достиг ли робот цели. helperUpdateMovingObstaclesKINOVACodeGen сценарий перемещает местоположения препятствий на основе временного шага.

options = nlmpcmoveopt;

for timestep=1:maxIters
    disp(['Calculating control at timestep ', num2str(timestep)]);
    % Optimize next trajectory point 
    if ~useMex
        options.Parameters = paras;
        [mv,options,info] = nlmpcmove(nlobj,x0,mv,[],[], options);
    else
        onlinedata.Parameters = paras; 
        [mv,onlinedata,info] = kinovaMex(x0',mv,onlinedata);
    end
    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 
    helperCheckGoalReachedKINOVACodeGen;
    if goalReached
        break;
    end
    % Update obstacle pose if it is moving
    if isMovingObst
        helperUpdateMovingObstaclesKINOVACodeGen;
    end
end

Calculating control at timestep 1
Calculating control at timestep 2
Calculating control at timestep 3
Calculating control at timestep 4
Calculating control at timestep 5
Calculating control at timestep 6
Calculating control at timestep 7
Calculating control at timestep 8

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);

Визуализация движения робота.

helperFinalVisualizerKINOVACodeGen;

Как и ожидалось, робот успешно движется по запланированной траектории и избегает препятствий.