使用KINOVA Gen3机械手生成规划和执行无碰撞轨迹的代码
这个例子展示了如何使用模型预测控制(MPC)来生成代码，以加速闭环无碰撞机器人轨迹的规划和执行。有关如何使用MPC进行机器人操纵器的运动规划的更多信息，请参见示例使用KINOVA Gen3机械手规划和执行无碰撞轨迹(机器人系统工具箱)．
代码生成的修改
要启用代码生成，请调整中的代码使用KINOVA Gen3机械手规划和执行无碰撞轨迹(机器人系统工具箱)按照下面的步骤操作。
-自定义成本函数:nlmpcCostFunctionKINOVACodeGen.m
-自定义不等式约束:nlmpcIneqConFunctionKINOVACodeGen.m
-状态功能:nlmpcModelKINOVACodeGen.m
—输出功能:nlmpcOutputKINOVACodeGen.m
-定制成本函数的雅可比矩阵:nlmpcJacobianCostKINOVACodeGen.m
-自定义不等式约束的雅可比矩阵:nlmpcJacobianConstraintKINOVACodeGen.m
-状态函数的雅可比矩阵:nlmpcJacobianModelKINOVACodeGen.m
-输出函数的雅可比矩阵:nlmpcJacobianOutputKINOVACodeGen.m
使用
参数的属性nlmpcmoveopt对象(如果是mex文件，则使用onlineData结构)向控制器传递在运行时(即在每个时间步中)或在连续运行之间可能更改的任何参数。例如，要根据每一个时间步的新传感器读数更新障碍物姿态，则当前障碍物姿态的值(posesNow)添加到参数个数细胞变量。同样，若要在线更新目标机器人姿态，则目标姿态(poseFinal)添加到参数列表。所有参数必须为数值。的更新值参数个数然后复制到参数的财产nlmpcmoveopt对象(如果nlmpcmove是使用)还是在onlineData结构(如果使用MEX文件)。使用
持续的要传入的变量不是数值的，也不是在运行时进行外部修改的。例如，rigidBodyTree(机器人系统工具箱)对象机器人的单元格数组collisionMesh(机器人系统工具箱)对象世界在第一次使用以下代码执行helper函数时创建并保存为持久变量。
持续的机器人的世界如果Isempty (robot) robot = loadrobot(“kinovaGen3”，“DataFormat”，“列”）;[world， ~] = helperCreateObstaclesKINOVA(posesNow);结束
确保辅助文件中使用的所有函数都支持代码生成。万博1manbetx有关支持代码生成的内置MATLAB函数的列表，请参阅此万博1manbetx页面．若要测试自定义函数是否支持代码生成，请使用万博1manbetx
coder.screener(万博1manbetx模型)．例如，试着跑步coder.screener(“nlmpcIneqConFunctionKINOVA”)．本示例要求loadrobot(机器人系统工具箱)而且checkCollision(机器人系统工具箱)万博1manbetx支持代码生成。使用
getCodeGenerationData而且buildMEX来生成的代码nlmpc对象使用指定的属性。取代nlmpcmove在原始示例中使用生成的MEX文件计算最优控制动作。
机器人描述和姿势
加载KINOVA Gen3刚体树(RBT)模型。
机器人=装载机器人“kinovaGen3”，“DataFormat”，“列”）;
得到关节的数量。
numjoint = nummel (homeConfiguration(robot));
指定连接末端执行器的机器人框架。
endEffector =“EndEffector_Link”；
指定初始和期望的末端执行器位姿。利用逆运动学来求解给定期望位姿的初始机器人构型。
初始末端执行器位姿taskInit = trvec2tform([[0.4 0 0.2]])*axang2tform([0 1 0 pi]);使用逆运动学计算当前机器人关节配置ik = inverseKinematics(“RigidBodyTree”,机器人);ik.SolverParameters.AllowRandomRestart = false;Weights = [1 1 1 1 1 1];currentRobotJConfig = ik(effffector, taskInit, weights, robot.homeConfiguration);currentRobotJConfig = wrapToPi(currentRobotJConfig);最终(期望的)末端执行器姿态。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向量:[x, y, z, phi, theta, psi]
碰撞网格和障碍
为了在控制过程中检查和避免碰撞，你必须设置碰撞世界作为一组碰撞对象。此示例使用collisionSphere(机器人系统工具箱)要避开的障碍物是物体。要计划使用静态而不是移动的对象，设置isMovingObst来假．
isMovingObst = true;
中的障碍物大小和位置初始化helperCreateMovingObstaclesKINOVACodeGenhelper函数。中添加更多的静态障碍，请添加碰撞对象世界数组中。
如果isMovingObst == true helperCreateMovingObstaclesKINOVACodeGen其他的posesNow = [0.4 0.4 0.25;0.3 0.3 0.4];结束[world, numObstacles] = helperCreateObstaclesKINOVACodeGen(posesNow);
想象机器人在初始配置时的样子。你也应该看到环境中的障碍。
x0 = [currentRobotJConfig'， zero (1, numjoint)];helperInitialVisualizerKINOVACodeGen;
指定与障碍物的安全距离。该值用于非线性MPC控制器的不等式约束函数。
safetyDistance = 0.01;
非线性模型预测控制器设计
可以利用该方法设计非线性模型预测控制器helperDesignNLMPCobjKINOVACodeGen助手文件，该文件将创建nlmpc控制器对象。要查看该文件，在MATLAB命令行中输入编辑helperDesignNLMPCobjKINOVACodeGen．
helperDesignNLMPCobjKINOVACodeGen;
闭环轨迹规划
在正确的初始条件下，模拟机器人最多50步。
初始化仿真参数
maxites = 50;u0 = 0 (1, numjoint);Mv = u0';时间= 0;goalReached = false;
生成代码nlmpc对象
useMex = true;buildMex = true;如果useMex [coredata, onlinedata] = getCodeGenerationData(nlobj,x0'，u0'，paras);如果buildMex (nlobj，“kinovaMex”、coredata onlinedata);结束结束
从非线性MPC生成MEX函数“kinovaMex”以加速模拟。代码生成成功。MEX功能“kinovaMex”成功生成。
初始化控件的数据数组
position = 0 (numjoint, maxites);locations (:，1) = x0(1: numjoint)';velocity = 0 (numjoint, maxites);velocity (:，1) = x0(numjoint +1:end)';accelerations = 0 (numjoint, maxites);加速度(:，1)= u0';timestamp = 0 (1, maxites);时间戳(:，1)=时间;
生成轨迹
使用生成的MEX文件kinovaMex而不是nlmpcmove函数加速闭环轨迹生成。使用参数指定在线轨迹生成选项onlineData．
每次迭代计算关节的位置、速度和加速度，以避免在向目标移动时遇到障碍。的helperCheckGoalReachedKINOVACodeGen脚本检查机器人是否达到了目标。的helperUpdateMovingObstaclesKINOVACodeGen脚本根据时间步长移动障碍物位置。
选项= nlmpcmoveopt;为步伐= 1:maxIters disp ([“计算时间步控制”num2str(步伐)]);优化下一个轨迹点如果~ useMex选项。参数= paras; [mv,options,info] = nlmpcmove(nlobj,x0,mv,[],[], options);其他的onlinedata。参数= paras; [mv,onlinedata,info] = kinovaMex(x0',mv,onlinedata);结束如果信息。ExitFlag < 0 disp(“未能计算出可行的轨迹。流产……”）打破；结束为下一次迭代更新状态和时间。x0 = info.Xopt(2，:);time = time + nlobj.Ts;%存储轨迹点locations (:，timestep+1) = x0(1: numjoint)';velocity (:，timestep+1) = x0(numjoint +1:end)';accelerations(:，timestep+1) = info.MVopt(2，:)';时间戳(timestep+1) =时间;检查目标是否达到helperCheckGoalReachedKINOVACodeGen;如果goalReached打破；结束如果障碍物在移动，则更新障碍物姿势。如果isMovingObst helperUpdateMovingObstaclesKINOVACodeGen;结束结束
计算timestep 1时刻控制计算timestep 2时刻控制计算timestep 3时刻控制计算timestep 4时刻控制计算timestep 5时刻控制计算timestep 6时刻控制计算timestep 7时刻控制计算timestep 8时刻控制计算
达到目标配置。
低保真度机器人控制与仿真实现计划轨迹
根据从计划中计算出的时间步长修剪轨迹向量。
tFinal = timestep+1;locations = locations (:，1:tFinal);速度=速度(:，1:tFinal);加速度=加速度(:，1:tFinal);timestamp = timestamp(:，1:tFinal);visTimeStep = 0.2;
使用一个jointSpaceMotionModel对象用计算转矩控制跟踪轨迹。的helperTimeBasedStateInputsKINOVA函数生成的导数输入ode15s用ODE求解器模拟机器人沿计算轨迹运动。
motionModel = jointSpaceMotionModel(“RigidBodyTree”,机器人);在仿真中利用低保真度模型控制机器人瞄准轨迹点。initState = [locations (:，1); velocity (:，1)];targetStates =[位置;速度;加速度]';[t,robotStates] = ode15s(@(t,state) helperTimeBasedStateInputsKINOVA(motionModel, timestamp, targetStates, t,state)， [timestamp(1):visTimeStep:timestamp(end)]， initState);
想象机器人的运动。
helperFinalVisualizerKINOVACodeGen;
正如预期的那样，机器人成功地沿着计划的轨迹移动，并避开了障碍物。
另请参阅
相关的话题
您也可以从以下列表中选择网站:
