KINOVA Gen3机械手无碰撞轨迹规划与执行

这个示例使用:

打开生活的脚本

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

机器人描述和姿势

加载KINOVA Gen3刚体树(RBT)模型。

机器人=装载机器人(“kinovaGen3”,“DataFormat”,“专栏”)；

获取关节的数量。

numJoints =元素个数(homeConfiguration(机器人));

指定末端执行器所在的机器人框架。

endEffector =“EndEffector_链接”;

指定初始和期望的末端执行器姿态。利用逆运动学求解给定期望姿态的初始机器人构型。

初始末端执行器位姿taskInit=trvec2tform（[[0.4 0 0.2]]）*axang2tform（[0 1 0 pi]）；使用逆运动学计算当前机器人关节构型本土知识= inverseKinematics (“刚体树”机器人）；ik.SolverParameters.AllowRandomRestart=false；权重=[1]；currentRobotJConfig=ik（endEffector、taskInit、权重、robot.homeConfiguration）；%IK解算器尊重关节限制，但对于那些具有无限长关节的关节%范围，则必须将它们包装为区间[-pi，pi]上的有限范围。%由于其他关节的边界已在此范围内，因此它是%只需在整个机器人配置上调用wrapToPi就足够了%而不仅仅是在无限范围的关节上。currentRobotJConfig = wrapToPi (currentRobotJConfig);最终(期望的)末端执行器姿态taskFinal=trvec2tform（[0.35 0.55 0.35]）*axang2tform（[0.1 0 pi]）；角度final=rotm2eul（taskFinal（1:3,1:3），“XYZ”)；poseFinal=[taskFinal（1:3,4）；anglesFinal']；%最终姿势的6x1矢量：[x，y，z，φ，θ，psi]

碰撞网格和障碍

为了在控制过程中检查和避免碰撞，你必须设置一个碰撞世界作为一组碰撞对象。此示例使用碰撞球对象作为要避免的障碍物。将以下布尔值更改为使用静态障碍物而不是移动障碍物进行规划。

IsMovingBST=真；

障碍物的大小和位置在下面的辅助函数中初始化。要添加更多的静态障碍，请在世界数组中。

helperCreateObstaclesKINOVA；

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

x0 = [currentRobotJConfig'， zeros(1,numJoints)]; / /当前节点helperInitialVisualizerKINOVA;

图中包含一个轴对象。轴对象包含11个面片、线条类型的对象。这些对象表示基本链接、肩部链接、半臂1链接、半臂2链接、前臂链接、腕表1链接、腕表2链接、手镯链接、EndEffector链接、肩部链接、半臂1链接、半臂2链接、前臂链接、腕表1链接、腕表2链接、腕表2链接k_网，手镯_链_网，底座_链_网。

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

safetyDistance = 0.01;

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

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

helperDesignNLMPCobjKINOVA;

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

采用双积分器对机器人关节模型进行描述。模型的状态是 $x = [Q, Q_{}^{˙}]$ ，其中7个关节位置由 $Q$ 它们的速度用 $Q_{}^{˙}$ .模型的输入为关节加速度 $U = Q_{}^{¨}$ 。模型的动力学由下式给出：

$\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 (Q (T)))}^{'} Q_{R} (P_{裁判} - P (Q (T))) + U^{'} (T) Q_{U} U (T) dt + {(P_{裁判} - P (Q (T)))}^{'} Q_{T} (P_{裁判} - P (Q (T))) + {\dot{Q}}^{'} (T) Q_{v} \dot{Q} (T)$

在这里 $P (Q (T))$ 变换关节位置 $Q (T)$ 使用正向运动学和转化, $P_{裁判}$ 表示所需的末端效应器姿势。

的矩阵 $Q_{R}$ , $Q_{U}$ , $Q_{T}$ , $Q_{v}$ 是常数权矩阵。

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

$D_{我, J} \geq D_{安全}$

在这里 $D_{我, J}$ 表示与目标的距离 $我$ -机器人的身体 $J$ -th障碍物，使用checkCollision.

在这个例子中, $我$ 属于 ${1., 2., 3., 4., 5., 6.}$ （不包括基座和末端执行器机器人主体），以及 $J$ 属于 ${1., 2.}$ (使用2个障碍)。

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

闭环轨迹规划

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

maxIters = 50;情况= 0(1、numJoints);mv =情况;时间= 0;goalReached = false;

初始化控制的数据数组。

位置=零（numJoints，maxIters）；位置（：，1）=x0（1:numJoints）”；速度=零（numJoints，maxIters）；速度（：，1）=x0（numJoints+1:end）”；加速度=零（numJoints，maxIters）；加速度（：，1）=u0'；时间戳=零（1，maxIters）；时间戳（：，1）=时间；

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

选择= nlmpcmoveopt;对于步伐= 1:maxIters disp (['在时间步计算控件'，num2str（timestep）]；%优化下一个轨迹点[mv,options,info] = nlmpcmove(nlobj,x0,mv，[]， options);如果信息。ExitFlag < 0'计算可行轨迹失败。正在中止…')打破;终止更新状态和下一次迭代的时间x0=info.Xopt（2，：）；时间=时间+nlobj.Ts；%存储轨迹点职位(:,步伐+ 1)= x0 (1: numJoints) ';速度(:,步伐+ 1)= x0 (numJoints + 1:结束);加速度(:,步伐+ 1)= info.MVopt(2:)”;时间戳(步伐+ 1)=时间;检查目标是否达到helperCheckGoalReachedKINOVA;如果进球打破;终止%更新移动中的障碍物姿势如果isMovingObst helperUpdateMovingObstaclesKINOVA;终止终止

在timestep 1计算控制

闲置变量未使用或自定义成本函数中的零加权。所有的限制都是困难的。

在timestep 2计算控制

闲置变量未使用或自定义成本函数中的零加权。所有的限制都是困难的。

在timestep 3计算控制

闲置变量未使用或自定义成本函数中的零加权。所有的限制都是困难的。

在时间步骤4计算控制

闲置变量未使用或自定义成本函数中的零加权。所有的限制都是困难的。

在时间步骤5计算控制

闲置变量未使用或自定义成本函数中的零加权。所有的限制都是困难的。

在时间步骤6计算控制

闲置变量未使用或自定义成本函数中的零加权。所有的限制都是困难的。

在时间步骤7计算控制

闲置变量未使用或自定义成本函数中的零加权。所有的限制都是困难的。

配置达到目标。

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

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

tFinal=时间步长+1；位置=位置（：，1:tFinal）；速度=速度（：，1:tFinal）；加速度=加速度（：，1:tFinal）；时间戳=时间戳（：，1:tFinal）；visTimeStep=0.2；

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

motionModel = jointSpaceMotionModel (“刚体树”,机器人);控制机器人到目标轨迹点的仿真使用低逼真度模型初始状态=[位置（：，1）；速度（：，1）]；目标状态=[位置；速度；加速度]；[t，机器人状态]=ode15s（@（t，状态）helperTimeBasedStateInputsKINOVA（运动模型，时间戳，目标状态，t，状态），...[时间戳（1）：visTimeStep:timestamp（end）]，initState）；

想象机器人的运动。

Helperfinalkinova；

图中包含一个轴对象。axis对象包含47个类型为patch, line的对象。这些对象代表base_link, Shoulder_Link, HalfArm1_Link, HalfArm2_Link, ForeArm_Link，腕1_link，腕2_link, Bracelet_Link, EndEffector_Link, Shoulder_Link_mesh, HalfArm1_Link_mesh, HalfArm2_Link_mesh, ForeArm_Link_mesh，腕1_link_mesh，腕2_link_mesh, Bracelet_Link_mesh, base_link_mesh。

舒尔曼等。带有顺序凸优化和凸碰撞检查的运动规划国际机器人研究杂志33.9(2014): 1251 - 1270。