KINOVA Gen3マニピュレーターを使用した衝突のない軌跡の計画と実行

この例では次を使用します:

この例では,非線形モデルの予測制御を使用して,エンドエフェクタの初期姿勢から目的の姿勢まで衝突のない閉ループのロボット軌跡を計画する方法を説明します。結果の軌跡は,ジョイント空間運動モデルと計算トルク制御を使用して実行されます。障害物は静的または動的にすることができ,プリミティブ(球,円柱,ボックス)またはカスタムメッシュのいずれかとして設定できます。

ロボットの記述と姿勢

KINOVA Gen3剛体ツリー(RBT)モデルを読み込みます。

机器人= loadrobot (“kinovaGen3”，“DataFormat”，“列”）;

ジョイントの数を取得します。

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

エンドエフェクタの属するロボット座標系を指定します。

endEffector =“EndEffector_Link”；

エンドエフェクタの初期姿勢と目的の姿勢を指定します。逆運動学を使用して,指定した目的の姿勢について初期のロボットコンフィギュレーションを求めます。

初始末端执行器位姿taskInit = trvec2tform([[0.4 0 0.2]])*axang2tform([0 1 0 pi]);使用逆运动学计算当前机器人关节构型本土知识= inverseKinematics (“RigidBodyTree”,机器人);ik.SolverParameters.AllowRandomRestart = false;Weights = [1 1 1 1 1];currentRobotJConfig = ik(endEffector, taskInit, 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”）;poseFinal = [taskFinal (1:3, 4); anglesFinal”);最终姿态的% 6x1向量:[x, y, z, phi, theta]

衝突メッシュと障害物

制御中に衝突をチェックして避けるには,衝突世界を一連の衝突オブジェクトとして設定しなければなりません。この例では,回避する障害物としてcollisionSphereオブジェクトを使用します。動く障害物ではなく静的な障害物を使用して計画するには,次の論理式を変更します。

isMovingObst = true;

障害物のサイズと位置は,次の補助関数で初期化されます。静的な障害物をさらに追加するには,配列世界に衝突オブジェクトを追加します。

helperCreateObstaclesKINOVA;

初期コンフィギュレーションでのロボットを可視化します。環境内に障害物も表示されます。

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

图中包含一个坐标轴。轴包含11个类型为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。

障害物からの安全距離を指定します。この値は非線形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$

したがって,非線形モデル予測コントローラー(nlobj14)には個の状態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 ））$ は順運動学とgetTransformを使用してジョイント位置 $问（ t ）$ をエンドエフェクタの座標系に変換します。 $p_{裁判}$ は,目的のエンドエフェクタ姿勢を表します。

行列 $问_{r}$ 、 $问_{u}$ 、 $问_{t}$ および $问_{v}$ は,定数の重み行列です。

衝突を避けるには,コントローラーが次の不等式制約を満たさなければなりません。

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

ここで, $d_{我， j}$ は, $我$ 番目のロボットボディから $j$ 番目の障害物までの距離を表し,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,options,info] = nlmpcmove(nlobj,x0,mv，[]， options);如果信息。ExitFlag < 0未能计算出可行的轨道。流产……”）打破；结束更新状态和下一次迭代的时间x0 = info.Xopt (2);time = time + nlobj.Ts;存储轨迹点百分比职位(:,步伐+ 1)= x0 (1: numJoints) ';速度(:,步伐+ 1)= x0 (numJoints + 1:结束);加速度(:,步伐+ 1)= info.MVopt(2:)”;时间戳(步伐+ 1)=时间;检查目标是否达到helperCheckGoalReachedKINOVA;如果goalReached打破；结束%更新移动中的障碍物姿势如果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 (“RigidBodyTree”,机器人);控制机器人到目标轨迹点的仿真使用低逼真度模型initState =[位置(:1)、速度(:1)];targetStates =(位置,速度,加速度)';[t,robotStates] = ode15s(@(t,state) helperTimeBasedStateInputsKINOVA(motionModel,timestamp,targetStates,t,state)，．..(时间戳(1):visTimeStep:时间戳(结束)],initState);

ロボットの動きを可視化します。

helperFinalVisualizerKINOVA;

图中包含一个坐标轴。轴包含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。

KINOVA Gen3マニピュレーターを使用した衝突のない軌跡の計画と実行

ロボットの記述と姿勢

衝突メッシュと障害物

非線形モデル予測コントローラーの設計

閉ループ軌跡の計画

ジョイント空間のロボットのシミュレーションと制御を使用した計画軌跡の実行

机器人系统工具箱ドキュメンテーション

サポート

MATLABおよび金宝app仿真软件による自律移動ロボット開発