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

此示例使用：

这个例子展示了如何使用非线性模型预测控制来规划从初始姿势到期望末端执行器姿势的闭环无碰撞机器人轨迹。使用带有计算扭矩控制的联合空间运动模型执行生成的轨迹。障碍可以是静态的，也可以是动态的，可以设置为基本体（球体、圆柱体、长方体）或自定义网格。

机器人描述和姿势

加载KINOVA Gen3刚体树（RBT）模型。

robot=装载机器人(“kinovaGen3”,'数据格式','列');

获取关节数。

numJoints=numel（homeConfiguration（robot））；

指定连接末端执行器的机器人框架。

末端效应器=“EndEffector_Link”;

指定初始和所需的末端效应器姿势。使用反向运动学求解给定所需姿势的初始机器人配置。

%初始末端效应器姿势taskInit=trvec2tform（[0.4 0 0.2]]）*axang2tforms（[0 1 0 pi]）；%使用反向运动学计算当前机器人关节配置ik=反向运动学(“刚体树”，机器人）；例如：。解算器参数。AllowRandomRestart=false；权重=[1 1 1 1 1 1]；currentRobotJConfig=ik（endEffector，taskInit，weights，robot.homeConfiguration）；%IK解算器尊重关节限制，但对于那些具有无限大的关节
%范围，它们必须被包装成区间[-pi，pi]上的有限范围。
%由于其他关节已在此范围内绑定，因此
%足以对整个机器人配置调用wrapToPi
%而不仅仅是在无限范围的关节上。currentRobotJConfig=wrapToPi（current机器人JConfig）；%最终（所需）末端效应器姿势taskFinal=trvec2表格（[0.35 0.55 0.35]）*axang2表格（+0 1 0 pi]）；角度最终值=rotm2eul（taskFinal（1:3,1:3），“XYZ”);poseFinal=[taskFinal（1:3,4）；角度Final’]；%最终姿势的6x1矢量：[x，y，z，phi，theta，psi]

碰撞网格和障碍物

要在控制期间检查并避免碰撞，必须设置碰撞世界作为一组碰撞对象。此示例使用碰撞球体对象作为要避免的障碍。将以下布尔值更改为使用静态而非移动障碍物进行规划。

isMovingObst=真；

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

helperCreateObstaclesKINOVA；

在初始配置时可视化机器人。你也应该看到环境中的障碍。

x0=[currentRobotJConfig'，零（1，numJoints）]；helperInitialVisualizerKINOVA；

图中包含轴对象。xlabel X，ylabel Y的axes对象包含11个patch，line类型的对象。一条或多条线仅使用标记显示其值。这些对象表示base_link、Shoulder_link、HalfArm1_link、HalfArm2_link，ForeArm_link，Wrist1_link和Wrist2_link，Bracelt_link，EndEffector_link。Shoulder-link_mesh，HalfArm2和link_mesh，手镯链接网格，基础链接网格。

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

安全距离=0.01；

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

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

helperDesignNLMPCobjKINOVA；

控制器的设计基于以下分析。优化解算器的最大迭代次数设置为5。明确设置关节位置和速度（状态）以及加速度（控制输入）的上下限。

机器人关节模型由双积分器描述。模型的状态为 $x个 = [q个, {q个}_{}^{˙}]$ ，其中7个接头位置表示为 $q个$ 它们的速度表示为 ${q个}_{}^{˙}$ 模型的输入是联合加速度 $单位 = {q个}_{}^{¨}$ 模型的动力学由下式给出

$\dot{x个} = [\begin{matrix} 0 & 我_{7} \\ 0 & 0 \end{matrix}] x + [\begin{matrix} 0 \\ 我_{7} \end{matrix}] 单位$

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

$年 = [\begin{matrix} 我_{7} & 0 \end{matrix}] x$ .

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

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

$J = \int_{0}^{T型} {({第页}_{裁判} - 第页 (q个 (t吨)))}^{'} 问_{第页} ({第页}_{裁判} - 第页 (q个 (t吨))) + {单位}^{'} (t吨) 问_{单位} 单位 (t吨) 日期 + {({第页}_{裁判} - 第页 (q个 (T型)))}^{'} 问_{t吨} ({第页}_{裁判} - 第页 (q个 (T型))) + {\dot{q个}}^{'} (T型) 问_{v（v）} \dot{q个} (T型)$

在这里， $第页 (q个 (t吨))$ 变换关节位置 $q个 (t吨)$ 使用正向运动学和获取转换、和 ${第页}_{裁判}$ 表示所需的末端效应器姿势。

矩阵 $问_{第页}$ , $问_{单位}$ , $问_{t吨}$ 、和 $问_{v（v）}$ 是恒重矩阵。

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

${d日}_{我, j} \geq {d日}_{安全的}$

在这里， ${d日}_{我, j}$ 表示与 $我$ -第个机器人身体 $j$ -第个障碍物，使用计算选中碰撞.

在这个例子中， $我$ 属于 ${1, 2, 三, 4, 5, 6}$ （不包括底座和末端执行器机器人本体），以及 $j$ 属于 ${1, 2}$ （使用2个障碍物）。

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

闭环轨迹规划

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

最大值=50；u0=零（1，numJoints）；mv=u0；时间=0；goalReached=false；

初始化控件的数据数组。

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

使用nlmpcm移动（模型预测控制工具箱）闭环轨迹生成功能。使用nlmpcmoveopt（模型预测控制工具箱）对象。每次迭代都会计算关节的位置、速度和加速度，以在关节向目标移动时避开障碍物。这个helper检查目标已达成KINOVA脚本检查机器人是否达到目标。这个helper更新移动障碍脚本根据时间步长移动障碍物位置。

选项=nlmpcmoveopt；对于时间步长=1:maxIters显示(['在时间步计算控件'，num2str（时间步）]）；%优化下一个轨迹点[mv，options，info]=nlmpcmove（nlobj，x0，mv，[]，[]）；如果信息。退出标志<0显示(“无法计算可行的轨迹。正在中止…')打破;结束
    %更新下一次迭代的状态和时间x0=信息。Xopt（2，：）；时间=时间+nlobj.Ts；%存储轨迹点位置（：，时间步+1）=x0（1:numJoints）'；速度（：，时间步+1）=x0（numJoints+1：结束）'；加速度（：，timestep+1）=信息。MVopt（2，：）'；timestamp（timestep+1）=时间；%检查目标是否实现帮助检查目标达到KINOVA；如果已达成目标打破;结束
    %如果障碍物正在移动，则更新障碍物姿势
    如果正在移动ObsthelperUpdateMovingObstaclesKINOVA；结束
结束

在时间步长1计算控制

在自定义成本函数中未使用或归零的松弛变量。所有约束都很难实现。

在时间步骤2计算控制

在自定义成本函数中未使用或归零的松弛变量。所有约束都很难实现。

在时间步骤3计算控制

在自定义成本函数中未使用或归零的松弛变量。所有约束都很难实现。

在时间步骤4计算控制

在自定义成本函数中未使用或归零的松弛变量。所有约束都很难实现。

在时间步骤5计算控制

在自定义成本函数中未使用或归零的松弛变量。所有约束都很难实现。

在时间步骤6计算控制

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

在时间步骤7计算控制

在自定义成本函数中未使用或归零的松弛变量。所有约束都很难实现。

已达到目标配置。

使用Joint-Space机器人仿真和控制执行计划轨迹

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

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

使用jointSpaceMotionModel（连接空间运动模型）用计算机扭矩控制跟踪轨迹。这个帮助程序基于时间的状态输入KINOVA函数生成的导数输入代码15用于建模计算出的机器人轨迹的函数。

motionModel=关节空间运动模型(“刚体树”，机器人）；%利用低维模型控制机器人到目标轨迹点的仿真initState=[位置（：，1）；速度（：，2）]；targetStates=[位置；速度；加速度]'；[t，robotStates]=ode15s（@（t，state）helperTimeBasedStateInputsKINOVA（运动模型，时间戳，目标状态，t，状态），...[时间戳（1）：visTimeStep:时间戳（结束）]，initState）；

可视化机器人的运动。

helperFinalVisualizerKINOVA；

图中包含轴对象。xlabel X，ylabel Y的axes对象包含47个patch，line类型的对象。一条或多条线仅使用标记显示其值。这些对象表示base_link、Shoulder_link、HalfArm1_link、HalfArm2_link，ForeArm_link，Wrist1_link和Wrist2_link，Bracelt_link，EndEffector_link。Shoulder-link_mesh，HalfArm2和link_mesh，手镯链接网格，基础链接网格。

[1] Schulman，J.等人，《顺序凸优化和凸碰撞检查的运动规划》国际机器人研究杂志33.9 (2014): 1251-1270.

另请参阅

jointSpaceMotionModel（连接空间运动模型）|nlmpc公司（模型预测控制工具箱）|contopptraj公司