使用KINOVA Gen3机械手计划和执行无碰撞轨迹

这个例子使用了:

打开实时脚本

这个例子展示了如何使用非线性模型预测控制来规划从初始到期望末端执行器姿态的闭环无碰撞机器人轨迹。所得到的轨迹是使用计算扭矩控制的关节空间运动模型来执行的。障碍物可以是静态的，也可以是动态的，可以设置为原语(球体、圆柱体、盒子)，也可以设置为自定义网格。

机器人描述及姿势

加载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 =逆运动学(“RigidBodyTree”,机器人);ik.SolverParameters.AllowRandomRestart = false;权重= [1 1 1 1 1 1];currentRobotJConfig = ik(enffector, taskkinit, weights, robot.homeConfiguration);IK解算器尊重关节极限，但对于那些关节有无穷%的范围，它们必须在间隔[-pi, pi]上被包装到一个有限的范围。由于其他关节已经在这个范围内，所以是%足以简单地在整个机器人配置上调用wrapToPi%而不是只在关节上无限范围。currentRobotJConfig = wrapToPi(currentRobotJConfig);最终(期望的)末端执行器姿态taskFinal = trvec2tform([0.35 0.55 0.35])*axang2tform([0 1 0 pi]);anglesFinal = rotm2eul(taskFinal(1:3,1:3)，“XYZ”）;posfinal = [taskFinal(1:3,4);anglesFinal'];最终姿态的% 6x1向量:[x, y, z，， theta, psi]

碰撞网格和障碍

为了检查和避免控制过程中的碰撞，必须设置碰撞世界作为一组碰撞对象。这个例子使用了collisionSphere要避开的障碍物。将下面的布尔值更改为计划使用静态障碍物而不是移动障碍物。

isMovingObst = true;

障碍物的大小和位置在下面的辅助函数中初始化。中添加碰撞对象可添加更多静态障碍物世界数组中。

helperCreateObstaclesKINOVA;

想象机器人在初始配置时的样子。你也应该看到环境中的障碍。

x0 = [currentRobotJConfig'， 0 (1, numjoint)];helperInitialVisualizerKINOVA;

指定与障碍物之间的安全距离。该值用于非线性MPC控制器的不等式约束函数。

safetyDistance = 0.01;

非线性模型预测控制器设计

您可以使用以下帮助文件设计非线性模型预测控制器，该文件将创建一个nlmpc(模型预测控制工具箱)控制器对象。要查看文件，请键入编辑helperDesignNLMPCobjKINOVA．

helperDesignNLMPCobjKINOVA;

控制器的设计基于以下分析。优化求解器的最大迭代次数设置为5。关节位置、速度(状态)和加速度(控制输入)的下界和上界是明确设置的。

采用双积分器对机器人关节模型进行描述。模型的状态是 $x ＝［问, 问_{}^{˙} ］$ ，其中7个关节位置表示为 $问$ 它们的速度表示为 $问_{}^{˙}$ ．模型的输入是关节加速度 $u ＝问_{}^{¨}$ ．模型的动力学由

$\dot{x} ＝［ \begin{matrix} 0 & 我_{7} \\ 0 & 0 \end{matrix} ］ x + ［ \begin{matrix} 0 \\ 我_{7} \end{matrix} ］ u$

在哪里 $我_{7}$ 表示 $7 \times 7$ 单位矩阵。模型的输出被定义为

$y ＝［ \begin{matrix} 我_{7} & 0 \end{matrix} ］ x$ ．

因此，非线性模型预测控制器(nlobj)有14个状态，7个输出，7个输入。

的代价函数nlobj是一个自定义的非线性代价函数，其定义方式类似于二次跟踪代价加上终端代价。

$J ＝ \int_{0}^{T} {（ p_{裁判} - p （问（ t ）））}^{”} 问_{r} （ p_{裁判} - p （问（ t ））） + u^{”} （ t ）问_{u} u （ t ） dt + {（ p_{裁判} - p （问（ T ）））}^{”} 问_{t} （ p_{裁判} - p （问（ T ））） + {\dot{问}}^{”} （ T ）问_{v} \dot{问} （ T ）$

在这里, $p （问（ t ））$ 变换关节位置 $问（ t ）$ 对末端执行器的机架进行正运动学分析getTransform, $p_{裁判}$ 表示所需的末端执行器姿态。

的矩阵 $问_{r}$ , $问_{u}$ , $问_{t}$ , $问_{v}$ 都是常权矩阵。

为了避免碰撞，控制器必须满足以下不等式约束。

$d_{我, j} \geq d_{安全}$

在这里, $d_{我, j}$ 的距离 $我$ -th机器人本体到 $j$ -th障碍，计算使用checkCollision．

在这个例子中， $我$ 属于 $｛ 1, 2, 3., 4, 5, 6 ｝$ (不包括基座和末端执行器机器人本体)，和 $j$ 属于 $｛ 1, 2 ｝$ (使用2个障碍物)。

为预测模型提供了状态函数、输出函数、代价函数和不等式约束的雅可比矩阵，提高了仿真效率。要计算不等式约束雅可比矩阵，请使用geometricJacobian函数和[1]的雅可比近似。

闭环轨迹规划

在正确的初始条件下，模拟机器人最多50步。

maxisters = 50;u0 = 0 (1, num关节);Mv = u0;时间= 0;goalReached = false;

初始化控件的数据数组。

位置=零(num关节，maxisters);position (:，1) = x0(1: num关节)';速度=零(numjoint, maxisters);速度(:，1)= x0(num关节+1:结束)';加速度=零(numjoint, maxisters);加速度(:，1)= u0';时间戳= 0 (1,maxisters);时间戳(:，1)=时间;

使用nlmpcmove(模型预测控制工具箱)闭环轨迹生成函数。属性指定轨迹生成选项nlmpcmoveopt(模型预测控制工具箱)对象。每次迭代都会计算关节的位置、速度和加速度，以避免它们在向目标移动时遇到障碍。的helperCheckGoalReachedKINOVA脚本检查机器人是否达到了目标。的helperUpdateMovingObstacles脚本根据时间步长移动障碍物位置。

选项= nlmpcmoveopt;为步伐= 1:maxIters disp ([“计算时间步长的控制”num2str(步伐)]);优化下一个轨迹点[mv,options,info] = nlmpcmove(nlobj,x0,mv，[]，[]， options);如果信息。ExitFlag < 0“未能计算出可行的轨迹。流产……”）打破；结束更新下一次迭代的状态和时间x0 = info.Xopt(2，:);time = time + nlobj.Ts;存储轨迹点position (:，timestep+1) = x0(1: num关节)';速度(:，timestep+1) = x0(num关节+1:结束)';加速度(:，timestep+1) = info.MVopt(2，:)';时间戳(timestep+1) =时间;检查目标是否达到helperCheckGoalReachedKINOVA;如果goalReached打破；结束如果障碍物在移动，更新障碍物的姿势如果isMovingObst helperUpdateMovingObstaclesKINOVA;结束结束

计算时间步骤1的控制

松弛变量未使用或自定义成本函数中的零权重。所有的约束都是困难的。

计算时间步骤2的控制

松弛变量未使用或自定义成本函数中的零权重。所有的约束都是困难的。

计算时间步骤3的控制

松弛变量未使用或自定义成本函数中的零权重。所有的约束都是困难的。

计算时间步骤4的控制

松弛变量未使用或自定义成本函数中的零权重。所有的约束都是困难的。

计算时间步骤5的控制

松弛变量未使用或自定义成本函数中的零权重。所有的约束都是困难的。

计算时间步6的控制

松弛变量未使用或自定义成本函数中的零权重。所有的约束都是困难的。

计算时间步骤7的控制

松弛变量未使用或自定义成本函数中的零权重。所有的约束都是困难的。

达到目标配置。

利用关节空间机器人仿真与控制执行计划轨迹

根据从计划中计算的时间步长修整轨迹向量。

tFinal = timestep+1;位置=位置(:，1:tFinal);速度=速度(:，1:tFinal);加速度=加速度(:，1:tFinal);时间戳=时间戳(:，1:tFinal);visTimeStep = 0.2;

使用一个jointSpaceMotionModel用计算转矩控制跟踪轨迹。的helperTimeBasedStateInputsKINOVA函数生成的导数输入ode15s对计算机器人轨迹建模的函数。

motionModel = jointSpaceMotionModel(“RigidBodyTree”,机器人);在仿真中使用低保真模型控制机器人到目标轨迹点。initState = [position (:，1); velocity (:，1)];targetStates =[位置;速度;加速度]';[t,robotStates] = ode15s(@(t,state) helperTimeBasedStateInputsKINOVA(motionModel,timestamp,targetStates,t,state)，.．.(时间戳(1):visTimeStep:时间戳(结束)],initState);

想象机器人的运动。

helperFinalVisualizerKINOVA;

[1]舒尔曼，J.，等。“带有连续凸优化和凸碰撞检查的运动规划。”国际机器人研究杂志33.9(2014): 1251-1270。