计划和执行无碰撞轨迹使用KINOVA Gen3操纵者

这个示例使用:

这个例子展示了如何从一个初始计划闭环机器人无碰撞轨迹末端执行器所需的姿势使用非线性模型预测控制。由此产生的轨迹执行使用关节空间运动模型计算转矩控制。障碍可以是静态或动态,可以设置为原语(球体、圆柱体框)或自定义网格。

机器人描述和姿势

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

机器人= loadrobot (“kinovaGen3”,“DataFormat”,“列”);

关节的数目。

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

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

endEffector =“EndEffector_Link”;

指定初始和期望的末端执行器构成。使用机器人逆运动学求解的初始配置所需的姿势。

%初始末端执行器构成taskInit = trvec2tform ([[0.4 0 0.2]]) * axang2tform([0 1 0π]);%计算当前机器人关节配置使用逆运动学本土知识= inverseKinematics (“RigidBodyTree”,机器人);ik.SolverParameters。AllowRandomRestart = false;重量= [1 1 1 1 1 1];本土知识currentRobotJConfig = (endEffector taskInit,重量、robot.homeConfiguration);%的动力学解算器方面联合限制,但是对于那些与无限的关节%的范围内,他们必须用一个有限的范围区间[-ππ]。%自其他关节已经有界在这个范围内,它是%足够简单地调用wrapToPi整个机器人配置%,而不是只在关节与无限的范围。currentRobotJConfig = wrapToPi (currentRobotJConfig);%的最后(期望)末端执行器构成taskFinal = trvec2tform (0.35 [0.55—0.35]) * axang2tform([0 1 0π]);anglesFinal = rotm2eul (taskFinal (1:3, 1:3),“XYZ”);poseFinal = [taskFinal (1:3, 4); anglesFinal”);% 6 x1向量为最后的姿势:[x, y, z,φ和θ,psi)

碰撞网格和障碍

在控制检查,避免碰撞,你必须设置一个碰撞世界作为一组碰撞对象。这个示例使用collisionSphere对象作为避免障碍。改变以下布尔计划使用静态的运动障碍。

isMovingObst = true;

障碍的大小和位置初始化以下helper函数。添加更多的静态障碍,增加碰撞中的对象世界数组中。

helperCreateObstaclesKINOVA;

想象机器人的初始配置。您应该看到环境中的障碍。

x0 = [currentRobotJConfig ', 0 (numJoints)];helperInitialVisualizerKINOVA;

图包含一个坐标轴对象。坐标轴对象包含11个补丁,类型的对象。这些对象代表base_link, Shoulder_Link、HalfArm1_Link HalfArm2_Link, ForeArm_Link, Wrist1_Link, Wrist2_Link, Bracelet_Link, EndEffector_Link, Shoulder_Link_mesh, HalfArm1_Link_mesh, HalfArm2_Link_mesh, ForeArm_Link_mesh, Wrist1_Link_mesh, Wrist2_Link_mesh, Bracelet_Link_mesh base_link_mesh。

指定一个安全距离的障碍。这个值是用于非线性不等式约束函数的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步骤与正确的初始条件。

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

初始化数据阵列控制。

位置= 0 (numJoints maxIters);职位(:1)= x0 (1: numJoints) ';速度= 0 (numJoints maxIters);速度(:1)= x0 (numJoints + 1:结束);加速度= 0 (numJoints maxIters);加速度(:1)=情况';时间戳= 0 (1、maxIters);时间戳(:1)=时间;

使用nlmpcmove(模型预测控制工具箱)函数闭环轨迹生成。使用一个指定轨迹生成选项nlmpcmoveopt(模型预测控制工具箱)对象。每次迭代计算的位置、速度和加速度的关节,避免障碍目标迈进。的helperCheckGoalReachedKINOVA脚本检查机器人已经达到我们的目标。的helperUpdateMovingObstacles脚本基于时间步移动障碍位置。

选择= nlmpcmoveopt;为步伐= 1:maxIters disp ([计算控制步伐的num2str(步伐)]);%下轨迹点优化(mv,选项,信息)= nlmpcmove (nlobj x0, mv,[][],选项);如果信息。ExitFlag < 0 disp (“未能计算出一个可行的轨迹。流产……”)打破;结束%更新状态和时间下一个迭代x0 = info.Xopt (2);时间=时间+ nlobj.Ts;%保存轨迹点职位(:,步伐+ 1)= x0 (1: numJoints) ';速度(:,步伐+ 1)= x0 (numJoints + 1:结束);加速度(:,步伐+ 1)= info.MVopt (2:)”;时间戳(步伐+ 1)=时间;%检查目标是否实现helperCheckGoalReachedKINOVA;如果goalReached打破;结束%更新障碍造成移动如果isMovingObst helperUpdateMovingObstaclesKINOVA;结束结束

计算控制步伐1

松弛变量未使用或zero-weighted定制成本函数。所有约束将是非常困难的。

计算控制步伐2

松弛变量未使用或zero-weighted定制成本函数。所有约束将是非常困难的。

计算控制步伐3

松弛变量未使用或zero-weighted定制成本函数。所有约束将是非常困难的。

计算控制步伐4

松弛变量未使用或zero-weighted定制成本函数。所有约束将是非常困难的。

计算控制步伐5

松弛变量未使用或zero-weighted定制成本函数。所有约束将是非常困难的。

计算控制步伐6

松弛变量未使用或zero-weighted定制成本函数。所有约束将是非常困难的。

计算控制步伐7

松弛变量未使用或zero-weighted定制成本函数。所有约束将是非常困难的。

计算控制步伐8

松弛变量未使用或zero-weighted定制成本函数。所有约束将是非常困难的。

配置达到目标。

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

修剪轨迹向量基于时间步长计算的计划。

tFinal =步伐+ 1;位置=位置(:,1:tFinal);速度=速度(:,1:tFinal);加速度=加速度(:,1:tFinal);时间戳=时间戳(:,1:tFinal);visTimeStep = 0.2;

使用一个jointSpaceMotionModel与computed-torque跟踪轨迹控制。的helperTimeBasedStateInputsKINOVA函数的导数输入生成ode15s函数模型计算出的机器人轨迹。

motionModel = jointSpaceMotionModel (“RigidBodyTree”,机器人);%控制机器人目标轨迹点使用低保真模型模拟initState =[位置(:1)、速度(:1)];targetStates =(位置,速度,加速度)';[t, robotStates] = ode15s (@ (t,状态)helperTimeBasedStateInputsKINOVA (motionModel、时间戳、targetStates t,状态),…(时间戳(1):visTimeStep:时间戳(结束)],initState);

想象机器人运动。

helperFinalVisualizerKINOVA;

图包含一个坐标轴对象。坐标轴对象包含52块类型的对象,线。这些对象代表base_link, Shoulder_Link、HalfArm1_Link HalfArm2_Link, ForeArm_Link, Wrist1_Link, Wrist2_Link, Bracelet_Link, EndEffector_Link, Shoulder_Link_mesh, HalfArm1_Link_mesh, HalfArm2_Link_mesh, ForeArm_Link_mesh, Wrist1_Link_mesh, Wrist2_Link_mesh, Bracelet_Link_mesh base_link_mesh。

舒尔曼[1],J。,等。“运动规划与相邻凸优化和凸碰撞检查。”国际机器人研究杂志》上33.9 (2014):1251 - 1270。