逆運動学による軌跡制御モデリング

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

このS金宝appimulinkの例では，指定された軌跡に沿って逆运动学ブロックがマニピュレ，タ，を駆動する方法を示します。目的の軌跡は，マニピュレ，タ，のエンドエフェクタ用の，間隔が密な一連の姿勢として指定されます。軌跡の生成と中間点の定義は,ピックアンドプレース操作,空間加速度や速度のプロファイルからの軌跡の計算,さらにはカメラやコンピュータービジョンを使用した主要な座標系の外部観察の模倣など多数のロボット工学アプリケーションを表します。軌跡は生成後，逆运动学ブロックを使用してジョ@ @ント空間の軌跡に変換されます。その後これを使用して，マニピュレタおよびコントロラのダナミクスをシミュレトできます。

モデルの概要

モデルを読み込んで構成を確認します。

open_system (“IKTrajectoryControlExample.slx”）;

モデルは次の4の主要な処理で構成されます。

タ，ゲット姿勢の生成
逆運動学
マニピュレタダナミクス
姿勢の測定

タ，ゲット姿勢の生成

このStateflowチャートでは,マニピュレーターの現在のオブジェクティブである中間点を選択します。マニピュレーターが現在のオブジェクティブの許容誤差内に達すると,チャートは次の中間点にターゲットを調整します。チャ，トはさらに，関数eul2tformを使用して，中間点の成分を同次変換に変換し，組み立てます。選択できる中間点がなくなると，チャ，トはシミュレ，ションを終了します。

逆運動学

逆運動学によって一連のジョesc escント角度が計算され，エンドエフェクタの目的の姿勢が生成されました。逆运动学をrigidBodyTreeモデルと共に使用して，エンドエフェクタのタ，ゲット姿勢を同次変換として指定します。解の位置および向きに対して相対許容誤差の制約に一連の重みを指定して,ジョイント位置の初期推定を行います。ブロックは，ブロックパラメ，タ，で指定されているrigidBodyTreeモデルから目的の姿勢を生成するジョ@ @ント位置のベクトルを出力します。解の滑らかな連続性を確保するために,前回のコンフィギュレーションの解が,ソルバーの開始位置として使用されます。最後のシミュレーションタイムステップ以降にターゲット姿勢が更新されていない場合は,これによって計算の冗長性も軽減します。

マニピュレタダナミクス

マニピュレタダナミクスは2のコンポネントで構成されます。トルク信号を生成するコントローラーと,これらのトルク信号からマニピュレーターのダイナミクスをモデル化するダイナミクスモデルです。この例のコントローラーは,マニピュレーターの逆ダイナミクスによって計算されたフィードフォワードコンポーネントと,エラーを修正するフィードバックPDコントローラーを使用します。マニピュレ，タ，のモデルは，rigidBodyTreeオブジェクトと連動する前进动力ブロックを使用します。ダイナミクスと可視化の手法をより洗練させるために,控制系统工具箱™ブロックセットおよびSimscape多体™のツールを前进动力ブロックの代わりに使用することを検討してください。

姿勢の測定

姿勢の測定では，マニピュレ，タ，モデルからジョ，(路径选择)セクションでフィ，ドバックとして使用する同次変換行列に変換します。

マニピュレ，タ，の定義

この例で使用するマニピュレーターは,考虑索耶™ロボットマニピュレーターです。このマニピュレ，タ，を記述するrigidBodyTreeオブジェクトは，importrobot机器人を使用してURDF(统一格式)描述からインポートされます。

将操作器导入为rigidBodyTree对象Sawyer = importrobot(“sawyer.urdf”）;索耶。DataFormat =“列”；定义末端执行器主体名称eeName =“right_hand”；定义机械手的关节数量num关节= 8;%可视化操作器显示(索耶);Xlim ([-1.00 1.50]) ylim([-1.00 1.50]);zlim ([-1.02 - 0.98]);视图([128.88 - 10.45]);

中間点の生成

この例でのマニピュレタの目的は，メジcoins.pngで検出されるコ@ @ンの境界を描画できるようになることです。まず，コ▪▪ンの境界を見▪▪けるために▪▪メ▪▪ジが処理されます。

I = imread(“coins.png”）;bwBoundaries = imread(“coinBoundaries.png”）;图subplot(1,2,1)“边界”，“紧”)标题(原始图像的) subplot(1,2,2) imshow(bwBoundaries，“边界”，“紧”)标题(“带边界检测的处理图像”）

图包含2个轴。标题为Original Image的坐标轴1包含一个Image类型的对象。标题为Processed Image with Boundary Detection的坐标轴2包含一个Image类型的对象。

@ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @ @デタはmatファルboundaryDataから読み込まれます。边界は细胞配列であり,各细胞には検出された単一の境界のピクセル座標を記述する配列が含まれています。このデータの生成方法をより包括的に確認するには,“イメージ内の境界トレース”(图像处理工具箱が必要)の例を参照してください。

负载boundaryData.mat边界谁边界

名称大小字节类属性边界10x1 25376单元格

このデータをワールド座標系にマッピングするには,イメージの位置と,ピクセル座標と空間座標の間のスケーリングを定義する必要があります。

%图像原点坐标imageOrigin = [0.4,0.2, 08];将像素转换为物理距离的比例因子刻度= 0.0015;

各点における目的のエンドエフェクタの向きのオ▪▪ラ▪▪角も定義しなければなりません。

eeOrientation = [0, pi, 0];

この例では，エンドエフェクタが@ # @メ，ジの平面に対して常に垂直になるように，向きが選択されます。

この情報が定義されたら，目的の座標とオ。中間点はそれぞれ6要素ベクトルとして表現され,最初の3要素は,ワールド座標系におけるマニピュレーターの目的のxyz位置に対応します。最後の3要素は，目的の向きのzyxオラ角に対応します。

$路标＝［ \begin{matrix} X & Y & Z & ϕ_{z} & ϕ_{y} & ϕ_{x} \end{matrix} ］$

中間点が連結されて，n行6列の配列を形成します。ここでnは軌跡内の姿勢の合計数です。配列内の各行は，軌跡内の中間点に対応します。

清除之前的路径点并开始构建路径点数组清晰的路点%从图像原点上方开始waypt0 = [imageOrigin + [0 0 .2]，eeOrientation];触摸图像的原点waypt1 = [imageOrigin,eeOrientation];插值每个元素以平滑运动到图像的原点为I = 1:6 interp = linspace(waypt0(I)，waypt1(I)，100);wayPoints(:，i) = interp';结束

合計で，10個のコescンがあります。簡略化と高速化のために,コインの小さいサブセットは,中間点に渡される合計数を制限することによってトレースできます。以下では，メジ内でnumTraces = 3。

定义要追踪的硬币数量numTraces = 3;组装用于边界跟踪的路点为i = 1:min(numtrace, size(边界，1))选择边界并映射到物理大小Segment = boundaries{i}*scale;在边界之间填充进近路点和升力路点的数据分段=[分段(1，:);段(:,);段(,)):;用于在边界之间移动的z偏移量段(1,3)= .02;段(end,3) = .02;%翻译到图像的原点cartesianCoord = imageOrigin + segment;%重复所需的方向，以匹配正在添加的路径点的数量eulerAngles = repmat(eeOrientation,size(segment,1)，1);将数据追加到前一个路径点的末尾wayPoints = [wayPoints;cartesianCoord eulerAngles);结束

この配列は，モデルに対する基本入力です。

モデルの設定

モデルを実行するには，いくかのパラメタを初期化しなければなりません。

初始化尺寸q0, t=0时机器人关节构型。这将%稍后将被第一个路径点替换。q0 = 0 (num关节，1);定义模拟的采样率。Ts = .01;在方向和上定义一个[1x6]相对权重向量%位置误差为逆运动学求解器。Weights = ones(1,6);将第一个路径点转换为齐次变换矩阵进行初始化initTargetPose = eul2tform(wayPoints(1,4:6));initTargetPose(1:3,end) = wayPoints(1,1:3)';求解q0，使操纵符从第一个路径点开始ik =逆运动学(“RigidBodyTree”索耶);[q0,solInfo] = ik(eeName,initTargetPose,weights,q0);

マニピュレ，タ，動作のシミュレ，ション

モデルをシミュレ，トするには，sim卡コマンドを使用します。モデルは出力デ，タセットjointDataを生成し，次の2のプロットで進行状況を表示します。

X - Y图には，マニピュレ，タ，のトレ，ス動作が上から下の順に表示されます。円の間の線は，あるコンの輪郭から次のコンへ，マニピュレタが遷移する際に発生します。
路径跟踪プロットは，進行状況を3dで可視化します。緑のドットはタ，ゲット位置を示します。赤のドットは,フィードバック制御を使用するエンドエフェクタによって達成された実際のエンドエフェクタ位置を示します。

关闭当前打开的数字关闭所有打开并模拟模型open_system (“IKTrajectoryControlExample.slx”）;sim卡(“IKTrajectoryControlExample.slx”）;

图2D Plot包含一个轴。标题为X Y Plot的轴包含类型为的对象。

图包含2个轴。标题为Waypoint Tracking的坐标轴1为空。坐标轴2包含3个line类型的对象。

結果の可視化

このモデルは、シミュレ、ション後に可視化に使用できる2のデ、タセットを出力します。ジョ▪▪ントコンフィギュレ▪▪ションはjointDataとして提供されます。ロボットのエンドエフェクタの姿勢はposeDataとして出力されます。

删除不必要的网格，以更快的可视化clearMeshes(索耶);%映射图像的数据[m,n] = size(I);[X,Y] = meshgrid(0:m,0:n);X = imageOrigin(1) + X*scale;Y = imageOrigin(2) + Y*scale;Z = 0(大小(X));Z = Z + imageOrigin(3);关闭所有打开的数字关闭所有初始化一个新的图形窗口图;集(gcf,“可见”，“上”）;绘制机器人初始位置显示(索耶,jointData (: 1) ');持有在初始化末端执行器绘图位置P = plot3(0,0,0，“。”）;经(X, Y, Z,我');%改变视角和轴视图(65,45)轴([-。25 1 -。25 .75 0 0.75])以10个样本的间隔迭代输出以可视化结果。为j = 1:10:length(jointData)显示机械手型号显示(索耶,jointData (j:) ',“帧”，“关闭”，“PreservePlot”、假);从齐次变换输出中获取末端执行器位置pos = poseData(1:3,4,j);为绘图更新末端执行器位置p. xdata = [p.]XData pos(1)]; p.YData = [p.YData pos(2)]; p.ZData = [p.ZData pos(3)];%更新数字drawnow结束