ロボットアームの最小加加速度の軌跡の計画

ライブスクリプトを開く

この例では、ロボットマニピュレーターの最小加加速度の多項式軌跡を計画する方法について説明します。例では、付属のロボットモデルを読み込み、障害物がある環境でのロボットモデルのパスを計画し、そのパスから最小加加速度の軌跡を生成し、生成された軌跡とロボットの動作を可視化する方法を示します。

ロボットモデルと環境の設定

この例では、7 自由度のロボットマニピュレーターである KUKA LBR iiwa のモデルを使用します。loadrobotを使用して、ロボットモデルをワークスペースにrigidBodyTreeオブジェクトとして読み込みます。コンフィギュレーション用の出力形式を "row" に設定します。

robot = loadrobot("kukaIiwa14",DataFormat="row");

ロボットの環境を生成します。衝突オブジェクトを作成し、ロボットのベースに対するそれらの姿勢を指定します。環境を可視化します。

env = {collisionBox(0.5,0.5,0.05,Pose=trvec2tform([0 0 -0.05])), ...
       collisionSphere(0.3,Pose=trvec2tform([0.1 0.2 0.8]))};
env{1}.Pose(3,end) = -0.05;
env{2}.Pose(1:3,end) = [0.1 0.2 0.8];

show(robot);
hold on
showCollisionArray(env);
axis([-1 1 -1 1 -0.1 1.75])
hold off

manipulatorRRT を使用したパスの計画

manipulatorRRTを使用してロボットモデルの RRT プランナーを作成します。パスを内挿しながら中間状態の数を増やすように ValidationDistance プロパティを設定します。

rrt = manipulatorRRT(robot,env);
rrt.ValidationDistance = 0.2;
rrt.SkippedSelfCollisions = "parent";

開始とゴールのコンフィギュレーションを指定します。

startConfig = [0.08 -0.65 0.05 0.02 0.04 0.49 0.04];
goalConfig =  [2.97 -1.05 0.05 0.02 0.04 0.49 0.04];

パスを計画します。RRT アルゴリズムのランダム性を考慮し、反復性のために rng シードを設定します。

rng(0)
path = plan(rrt,startConfig,goalConfig);

パスを内挿し、ウェイポイントを取得します。

interpPath = interpolate(rrt,path);
wpts = interpPath';

最小加加速度の多項式軌跡の生成

プランナーはパスを順序付きの一連のウェイポイントとして返します。それらをロボットに渡すには、それらを通る軌跡を最初に判別する必要があります。関数minjerkpolytrajは、指定されたすべてのウェイポイントを通過する最小加加速度の滑らかな軌跡を作成します。

ロボットアームがウェイポイントに到着する時間の初期推定を与えます。

initialGuess = linspace(0,size(wpts,2)*0.2,size(wpts,2));

軌跡の推定中に取得するサンプルの数を指定します。

numSamples = 100;

最小加加速度の多項式軌跡を計算します。

[q,qd,qdd,qddd,pp,tpts,tSamples] = minjerkpolytraj(wpts,initialGuess,numSamples);

ジョイントの軌跡とウェイポイントの可視化

時間の経過に伴う軌跡とウェイポイントをプロットします。

minJerkPath = q';
figure
plot(tSamples,q)
hold on
scatter(tpts,wpts,40)
title("Joint Trajectories and Waypoints")
xlabel("Time")
ylabel("Joint Angle (rad)")

Figure contains an axes object. The axes object with title Joint Trajectories and Waypoints, xlabel Time, ylabel Joint Angle (rad) contains 14 objects of type line, scatter.

ロボットの動作の可視化

オブジェクト関数showを使用して結果の動作をアニメーション化します。この可視化で高速更新を有効にすることで、滑らかなアニメーションが得られます。

figure
ax = show(robot,startConfig);
hold on
showCollisionArray(env);
axis([-1 1 -1 1 -0.1 1.75])
title(["Animation of Robot Arm Following","Minimum Jerk Trajectory"])
for i = 1:size(minJerkPath,1)
    show(robot,minJerkPath(i,:),PreservePlot=false,FastUpdate=true);
    drawnow;
end
hold off

Figure contains an axes object. The axes object with title Animation of Robot Arm Following Minimum Jerk Trajectory, xlabel X, ylabel Y contains 31 objects of type patch, line.

ロボット アームの最小加加速度の軌跡の計画