2 링크 매니퓰레이터의 궤도 추적 제어
2링크 매니퓰레이터
소개
본 기사는 제어 공학 Advent Calendar 2018 22일째가 됩니다.
올해는 여러분 제대로 준비된 기사가 많은 것 같습니다.
일단 게시물의 장애물을 낮추고 싶습니다.
이번에는 로봇 암 중 수평 2 링크 암 (2 링크 매니퓰레이터)
실제로 동작시켜, 매니퓰레이터의 궤도 추종 제어를 실천했습니다.
하드웨어 구성
사용한 로봇 암은 그림과 같습니다. 모 시판 로봇의 팔을 개조하여
사용하고 있습니다. 서보 모터에는 KRS-3204를 사용하고 있습니다.
콘도 과학의 서보 모터는 시리얼 통신에 의해 현재의 위치도 얻을 수 있기 때문에,
PWM에서 서보를 사용하는 경우에 비해 여러가지 놀 수 있습니다.
모델 설정
2 링크 매니퓰레이터를 그림과 같이 설정했습니다.
여기서 향후 해설을 위해 고정 된 서보 모터
서보 1, 서보 1에 연결된 암을 링크 1로 합니다. 또한 링크 1의 끝에
고정한 서보 모터를 서보 2, 암 선단의 링크를 링크 2로 합니다.
순운동학
전 그림과 같이 모델을 정의했으므로, 우선은 순운운동학으로부터 구해 보겠습니다.
순 운동학은 이번이면 서보 1, 2의 각도를 주면 링크 2의 선단 위치가
알 수 있는 식입니다.
우선, 링크 1의 선단의 위치를 구합니다. 삼각 함수를 사용하여 다음과 같이 됩니다.
x=L_1 cos\theta_1 + L_2cos(\theta_1+\theta_2)\\
y=L_1 sin\theta_1 + L_2sin(\theta_1+\theta_2)
以上で今回制御したいリンク２の先端位置がわかります。
順運動学による式は制御には直接用いませんが、MATLAB等で
シミュレーションを行う時に必要となったりします。
역운동학(해석해를 이용)
逆運動学では、リンク２の先端の位置を指定して、サーボ１，２の角度を得ます。
まず、サーボ１の角度$theta_1$について求めます。式変形の過程を示します。
移項します。
x-L_1 cos\theta_1=L_2cos(\theta_1+\theta_2)\\
y-L_1 sin\theta_1=L_2sin(\theta_1+\theta_2)
両辺を２乗します。
x^2-2xL_1 cos\theta_1+L_1^2cos^2\theta=L_2^2cos^2(\theta_1+\theta_2)\\
y^2-2yL_1 sin\theta_1+L_1^2sin^2\theta=L_2^2sin^2(\theta_1+\theta_2)
２つの式を足し合わせて整理します。
x^2+y^2-2xL_1 cos\theta_1-2yL_1 sin\theta_1+L_1^2(cos^2\theta+sin^2\theta)\\
=L_2^2(cos^2(\theta_1+\theta_2)+sin^2(\theta_1+\theta_2))\\
\therefore
x^2+y^2-2L_1 (xcos\theta_1+ ysin\theta_1) +L_1^2=L_2^2
$cos\theta_1+ sin\theta_1$について整理します。
xcos\theta_1+ ysin\theta_1 =\frac{x^2+y^2+L_1^2-L_2^2}{2L_1}
左辺について三角関数の合成で$cos$にまとめます。
\sqrt{x^2+y^2}cos(\theta_1+\gamma)=\frac{x^2+y^2+L_1^2-L_2^2}{2L_1}\\
(tan\gamma = \frac{y}{x})
あとは逆余弦を使って$theta_1$について解きます。
\theta_1=-\gamma\pm acos(\frac{x^2+y^2+L_1^2-L_2^2}{2L_1\sqrt{x^2+y^2}})\\
(tan\gamma = \frac{y}{x})
以上で$theta_1$については解けました。次に$theta_2$について解きます。
同様にまず、順運動学の式を基に移項します。
x-L_1 cos\theta_1=L_2cos(\theta_1+\theta_2)\\
y-L_1 sin\theta_1=L_2sin(\theta_1+\theta_2)
ここで、２つ目の式を１つ目の式で割ります。
\frac{y-L_1 sin\theta_1}{x-L_1 cos\theta_1}
=\frac{L_2sin(\theta_1+\theta_2)}{L_2cos(\theta_1+\theta_2)}\\
\therefore
\frac{y-L_1 sin\theta_1}{x-L_1 cos\theta_1}=tan(\theta_1+\theta_2)
あとは逆関数で求めておしまいです。
\theta_1+\theta_2=atan(\frac{y-L_1 sin\theta_1}{x-L_1 cos\theta_1})\\
\therefore
\theta_2=-\theta_1+atan(\frac{y-L_1 sin\theta_1}{x-L_1 cos\theta_1})
以上をまとめると，逆運動学を解析的にもとめた結果の一例は次のようになります。
(求め方によって他の表記になる場合もあるようです)
\theta_1=-\gamma\pm acos(\frac{x^2+y^2+L_1^2-L_2^2}{2L_1\sqrt{x^2+y^2}})\\
\theta_2=-\theta_1+atan(\frac{y-L_1 sin\theta_1}{x-L_1 cos\theta_1})\\
(tan\gamma = \frac{y}{x})
この時，リンク1に関しては２つの角度が計算されますが，ハードウェアの諸事情から
マイナス側の角度を採用しました．そのため，逆運動学の解析回として次式を利用しました。
\theta_1=-\gamma- acos(\frac{x^2+y^2+L_1^2-L_2^2}{2L_1\sqrt{x^2+y^2}})\\
\theta_2=-\theta_1+atan(\frac{y-L_1 sin\theta_1}{x-L_1 cos\theta_1})\\
(tan\gamma = \frac{y}{x})
역 운동학 (야코비 행렬을 이용)
今回は２リンクマニピュレータのため解析的にも解が求まります。しかし、自由度が
増加するにつれ解析的に逆運動学の式を求められないことがあります。そこで、
数値計算で解いていこうという方法もあります。
まず、順運動学で出てきた式の両辺を時間で微分します．
\dot x=-(L_1 sin\theta_1 + L_2 sin(\theta_1 + \theta_2))\dot \theta_1
-L_2 sin(\theta_1 + \theta_2)\\
\dot y=(L_1 cos\theta_1 +L_2 cos(\theta_1 + \theta_2))\dot \theta_1
+L_2 cos(\theta_1 + \theta_2)
次に，これを行列式を用いて表わすと，
\begin{pmatrix}
\dot x \\ \dot y
\end{pmatrix}
=
\begin{pmatrix}
-L_1 sin\theta_1 - L_2 sin(\theta_1 + \theta_2) &
-L_2 sin(\theta_1 + \theta_2)\\
L_1 cos\theta_1 +L_2 cos(\theta_1 + \theta_2) &
+L_2 cos(\theta_1 + \theta_2)
\end{pmatrix}
\begin{pmatrix}
\dot \theta_1 \\ \dot \theta_2
\end{pmatrix}
となります．この2×2の行列部分をヤコビ行列と呼ぶそうです．
そこで，位置$p$，角度$\theta$，ヤコビ行列$J$について下記のように定義します．
p=\begin{pmatrix}
x \\ y \end{pmatrix}
\\
\theta = \begin{pmatrix}
\theta_1 \\\theta_2 \end{pmatrix}
\\
J=\begin{pmatrix}
-L_1 sin\theta_1 - L_2 sin(\theta_1 + \theta_2) &
-L_2 sin(\theta_1 + \theta_2)\\
L_1 cos\theta_1 +L_2 cos(\theta_1 + \theta_2) &
+L_2 cos(\theta_1 + \theta_2)
\end{pmatrix}
よって，
\dot p = J \dot \theta
となります．この式は角速度と直線速度の関係式ともみれます。
ここでヤコビ行列$J$が逆行列を持つと仮定すると、
\dot \theta = J^{-1} \dot p \\
\therefore \frac{\delta \theta}{\delta t} = J^{-1} \frac{\delta p }{\delta t}
となります．
このとき十分に小さい幅でこの計算を行うとすれば，
\delta \theta = J^{-1} \delta p
と考えられます．そこで，$\theta$の初期値さえ分かれば後は，目標となる位置から，
与えるべき角度を求められます．具体的な計算ステップは次のようになります．
サーボに与えるべき角度を得ます．
1. $\theta$，$p$の初期値を設定する．
2. 目標軌道を決める(直線，円etc...)
3. 目標軌道を分割する．
4. 現在位置を分割した次の点に動かす($\delta p_n$が計算される)
5. 現在の角度を以下の式で更新する。$\theta_n = \theta_{n-1} + J_n^{-1} \delta p_n $
6. 4,5ステップを繰り返す．
このようにして解析的な解が求められない場合でも逆運動学計算を行うことができます．
MATLAB을 이용한 시뮬레이션
実際にMATLABを用いてシミュレーションしてみます。
実際のコードもついでに掲載しておきます。
注意点として、atan関数とatan2関数では全く異なる結果が出ます。
atanでは引数が一つであり、象限が不明なため$\pm \pi/2$の範囲で返されるのに対して
atan2では引数が２つあり、ちゃんと象限が分かるため，$\pm \pi$の範囲で返してくれます。
この点に気付くのに少し時間がかかってしまいました．
직선 궤도에 추종
직선 궤도에 추종하는 모습입니다. 
암의 궤적은 그림과 같이 되었습니다.
원 궤도에 추종
원선 기동에의 추종의 모습입니다. 이쪽도 이해하기 어렵지만 원을 그릴 수 있습니다.
해석 해를 이용한 경우와 야코비 행렬을 이용한 경우의 비교
애니메이션을 이용해 양자를 비교할 수 있으면 좋았습니다만,
불행히도 차이가 거의 없었기 때문에 각 서보의 각도 그래프를 나타냅니다.
약간 야코비 행렬을 이용한 경우 쪽이 오차가 쌓여 있는 것을 알 수 있습니다.
이것이 분할 점수의 문제인지, 그 이외의 무엇인가의 원인은 조사할 수 없습니다.
실제 기계에 적용
시뮬레이션에서 잘 작동한다는 것을 알았으므로 여기도
실제로 실기에 적용해 보았습니다. 동영상으로 전하는지 미묘하지만,
매우 굉장한 동작이되고 있습니다. 원인으로는 명령 간격이 길다.
보간 거리가 길고 목표 값으로 위치 만 제공하고 내부 서보 시스템에서
그 위치에서 속도가 0이 되도록 제어되고 있는 것 등이 생각됩니다.
지령주기를 고속화하거나 뭐든지 하면 부드러워질지도 모르지만,
Advent Calender의 사정상 여기까지로 용서해 주세요.
결론
잘 정리되어 있지 않고, 또 다른 투고 기사(Advent Calender 2018 제어 공학)에
비교해 얇은 내용이 되어 버렸던 것 사과 말씀드립니다.
참고 코드
MATLAB
function arm_inv_arm_angle
format compact;
clear all;
close all;
state_servo1=struct('angle',0,'length',0.085);
state_servo2=struct('angle',0,'length',0.085);
f = figure('Position',[1300,400,600,600]);
ax = axes('Parent',f,'position',[0.1 0.1 0.8 0.8]);
h=plot(0,0);hold on;pbaspect([1 1 1]); %表示アスペクト比を1:1
axis([-0.2,0.2,-0.2,0.2]);
% 角度で入力
% state_servo1.angle=deg2rad(-22.9094);
% state_servo2.angle=deg2rad(-68.5216);
% plot_arm(f,state_servo1,state_servo2)
% 逆運動学で計算してみる
% % ref=gen_servo_angle(0.2, 0.0)
% state_servo1.angle =ref(1);
% state_servo2.angle =ref(2);
% plot_arm(f,state_servo1,state_servo2)
% 逆運動学で直線を出す
y_ref=-0.05:0.005:0.05;
x_ref=ones(1,length(y_ref)).*0.15;
% 逆運動学で円を出す
% th=linspace(0,2*pi,100);
% R=0.01;
% x_ref=sin(th).*R +0.15;
% y_ref=cos(th).*R +0;
% アニメーション実行
t=0:Ts:(length(x_ref)-1)*Ts;
data_servo1_anlge=0;data_servo2_anlge=0;
data_servo1_anlge_J=0;data_servo2_anlge_J=0;
JacobianTheta=0;last_p=0; %ヤコビ行列で計算するとき
for i=1:length(x_ref)
% 解析解を用いた場合
ref=gen_servo_angle(x_ref(i),y_ref(i));
% ヤコビ行列を用いた場合
if i==1
last_p=[x_ref(1);y_ref(1)];
JacobianTheta=[ref(1);ref(2)];
else
J=JacobianMatrix(state_servo1.angle,state_servo2.angle,...
state_servo1.length,state_servo2.length);
delta_p=[x_ref(i);y_ref(i)]-last_p;
last_p = [x_ref(i);y_ref(i)];
JacobianTheta = JacobianTheta + J\ delta_p;
end
% ヤコビ行列でやる場合は下のコメントアウトを解除
% ref = JacobianTheta';
state_servo1.angle =ref(1);
state_servo2.angle =ref(2);
data_servo1_anlge(i)=ref(1);
data_servo2_anlge(i)=ref(2);
data_servo1_anlge_J(i)=JacobianTheta(1);
data_servo2_anlge_J(i)=JacobianTheta(2);
plot_arm(f,state_servo1,state_servo2)
pause(20/1000);
end
% 角度情報を見てみる
figure();
plot(t,rad2deg(data_servo1_anlge),t,rad2deg(data_servo2_anlge),...
t,rad2deg(data_servo1_anlge_J),t,rad2deg(data_servo2_anlge_J));
legend('servo1','servo2','servo1_J','servo2_J')
% ヤコビ行列を求める
function J=JacobianMatrix(theta1,theta2,L1,L2)
J=[-L1 * sin(theta1)-L2*sin(theta1+theta2) ,-L2*sin(theta1+theta2);
L1*cos(theta1)+L2*cos(theta1+theta2), L2*cos(theta1+theta2)];
end
function plot_arm(f,servo1,servo2)
figure(f);
cla; %この行をコメントアウトするとアームのステップごとの位置が見れる
% arm 1st
plot([0.0 , servo1.length * cos(servo1.angle)],...
[0.0 ,servo1.length * sin(servo1.angle)],'b');
% arm 2nd
plot([servo1.length * cos(servo1.angle) ,...
servo1.length * cos(servo1.angle)...
+ servo2.length * cos(servo1.angle + servo2.angle)],...
[servo1.length * sin(servo1.angle) ,...
servo1.length * sin(servo1.angle)...
+ servo2.length * sin(servo1.angle + servo2.angle)],'r');
end
function theta=gen_servo_angle(x,y)
L1=state_servo1.length;
L2=state_servo2.length;
% ここで+-の候補が出てくる．
theta(1)=atan2(y,x)...
-acos( (x^2 + y^2 + L1^2 - L2^2)/(2*L1 * sqrt(x^2+y^2)) );
% +acos( (x^2 + y^2 + L1^2 - L2^2)/(2*L1 * sqrt(x^2+y^2)) );
theta(2)=-theta(1) + atan2((y-L1*sin(theta(1))) , (x-L1*cos(theta(1))));
end
end
참고문헌
[1] http://www-bs.ss.oka-pu.ac.jp/inoue/class/robot/로봇 공학.pdf
[2]하야카와 쿄히로, 櫟 히로아키, 야노 준히코 기계계 교과서 시리즈 22 로봇 공학
Reference
이 문제에 관하여(2 링크 매니퓰레이터의 궤도 추적 제어), 우리는 이곳에서 더 많은 자료를 발견하고 링크를 클릭하여 보았다
https://qiita.com/fumiya_sato/items/9167f12bf182fa585968
텍스트를 자유롭게 공유하거나 복사할 수 있습니다.하지만 이 문서의 URL은 참조 URL로 남겨 두십시오.
우수한 개발자 콘텐츠 발견에 전념
(Collection and Share based on the CC Protocol.)
x=L_1 cos\theta_1 + L_2cos(\theta_1+\theta_2)\\
y=L_1 sin\theta_1 + L_2sin(\theta_1+\theta_2)
シミュレーションを行う時に必要となったりします。
まず、サーボ１の角度$theta_1$について求めます。式変形の過程を示します。
移項します。
x-L_1 cos\theta_1=L_2cos(\theta_1+\theta_2)\\
y-L_1 sin\theta_1=L_2sin(\theta_1+\theta_2)
x^2-2xL_1 cos\theta_1+L_1^2cos^2\theta=L_2^2cos^2(\theta_1+\theta_2)\\
y^2-2yL_1 sin\theta_1+L_1^2sin^2\theta=L_2^2sin^2(\theta_1+\theta_2)
x^2+y^2-2xL_1 cos\theta_1-2yL_1 sin\theta_1+L_1^2(cos^2\theta+sin^2\theta)\\
=L_2^2(cos^2(\theta_1+\theta_2)+sin^2(\theta_1+\theta_2))\\
\therefore
x^2+y^2-2L_1 (xcos\theta_1+ ysin\theta_1) +L_1^2=L_2^2
xcos\theta_1+ ysin\theta_1 =\frac{x^2+y^2+L_1^2-L_2^2}{2L_1}
\sqrt{x^2+y^2}cos(\theta_1+\gamma)=\frac{x^2+y^2+L_1^2-L_2^2}{2L_1}\\
(tan\gamma = \frac{y}{x})
\theta_1=-\gamma\pm acos(\frac{x^2+y^2+L_1^2-L_2^2}{2L_1\sqrt{x^2+y^2}})\\
(tan\gamma = \frac{y}{x})
同様にまず、順運動学の式を基に移項します。
x-L_1 cos\theta_1=L_2cos(\theta_1+\theta_2)\\
y-L_1 sin\theta_1=L_2sin(\theta_1+\theta_2)
\frac{y-L_1 sin\theta_1}{x-L_1 cos\theta_1}
=\frac{L_2sin(\theta_1+\theta_2)}{L_2cos(\theta_1+\theta_2)}\\
\therefore
\frac{y-L_1 sin\theta_1}{x-L_1 cos\theta_1}=tan(\theta_1+\theta_2)
\theta_1+\theta_2=atan(\frac{y-L_1 sin\theta_1}{x-L_1 cos\theta_1})\\
\therefore
\theta_2=-\theta_1+atan(\frac{y-L_1 sin\theta_1}{x-L_1 cos\theta_1})
(求め方によって他の表記になる場合もあるようです)
\theta_1=-\gamma\pm acos(\frac{x^2+y^2+L_1^2-L_2^2}{2L_1\sqrt{x^2+y^2}})\\
\theta_2=-\theta_1+atan(\frac{y-L_1 sin\theta_1}{x-L_1 cos\theta_1})\\
(tan\gamma = \frac{y}{x})
マイナス側の角度を採用しました．そのため，逆運動学の解析回として次式を利用しました。
\theta_1=-\gamma- acos(\frac{x^2+y^2+L_1^2-L_2^2}{2L_1\sqrt{x^2+y^2}})\\
\theta_2=-\theta_1+atan(\frac{y-L_1 sin\theta_1}{x-L_1 cos\theta_1})\\
(tan\gamma = \frac{y}{x})
増加するにつれ解析的に逆運動学の式を求められないことがあります。そこで、
数値計算で解いていこうという方法もあります。
\dot x=-(L_1 sin\theta_1 + L_2 sin(\theta_1 + \theta_2))\dot \theta_1
-L_2 sin(\theta_1 + \theta_2)\\
\dot y=(L_1 cos\theta_1 +L_2 cos(\theta_1 + \theta_2))\dot \theta_1
+L_2 cos(\theta_1 + \theta_2)
\begin{pmatrix}
\dot x \\ \dot y
\end{pmatrix}
=
\begin{pmatrix}
-L_1 sin\theta_1 - L_2 sin(\theta_1 + \theta_2) &
-L_2 sin(\theta_1 + \theta_2)\\
L_1 cos\theta_1 +L_2 cos(\theta_1 + \theta_2) &
+L_2 cos(\theta_1 + \theta_2)
\end{pmatrix}
\begin{pmatrix}
\dot \theta_1 \\ \dot \theta_2
\end{pmatrix}
そこで，位置$p$，角度$\theta$，ヤコビ行列$J$について下記のように定義します．
p=\begin{pmatrix}
x \\ y \end{pmatrix}
\\
\theta = \begin{pmatrix}
\theta_1 \\\theta_2 \end{pmatrix}
\\
J=\begin{pmatrix}
-L_1 sin\theta_1 - L_2 sin(\theta_1 + \theta_2) &
-L_2 sin(\theta_1 + \theta_2)\\
L_1 cos\theta_1 +L_2 cos(\theta_1 + \theta_2) &
+L_2 cos(\theta_1 + \theta_2)
\end{pmatrix}
\dot p = J \dot \theta
ここでヤコビ行列$J$が逆行列を持つと仮定すると、
\dot \theta = J^{-1} \dot p \\
\therefore \frac{\delta \theta}{\delta t} = J^{-1} \frac{\delta p }{\delta t}
\delta \theta = J^{-1} \delta p
与えるべき角度を求められます．具体的な計算ステップは次のようになります．
サーボに与えるべき角度を得ます．
1. $\theta$，$p$の初期値を設定する．
2. 目標軌道を決める(直線，円etc...)
3. 目標軌道を分割する．
4. 現在位置を分割した次の点に動かす($\delta p_n$が計算される)
5. 現在の角度を以下の式で更新する。$\theta_n = \theta_{n-1} + J_n^{-1} \delta p_n $
6. 4,5ステップを繰り返す．
実際のコードもついでに掲載しておきます。
注意点として、atan関数とatan2関数では全く異なる結果が出ます。
atanでは引数が一つであり、象限が不明なため$\pm \pi/2$の範囲で返されるのに対して
atan2では引数が２つあり、ちゃんと象限が分かるため，$\pm \pi$の範囲で返してくれます。
この点に気付くのに少し時間がかかってしまいました．
function arm_inv_arm_angle
format compact;
clear all;
close all;
state_servo1=struct('angle',0,'length',0.085);
state_servo2=struct('angle',0,'length',0.085);
f = figure('Position',[1300,400,600,600]);
ax = axes('Parent',f,'position',[0.1 0.1 0.8 0.8]);
h=plot(0,0);hold on;pbaspect([1 1 1]); %表示アスペクト比を1:1
axis([-0.2,0.2,-0.2,0.2]);
% 角度で入力
% state_servo1.angle=deg2rad(-22.9094);
% state_servo2.angle=deg2rad(-68.5216);
% plot_arm(f,state_servo1,state_servo2)
% 逆運動学で計算してみる
% % ref=gen_servo_angle(0.2, 0.0)
% state_servo1.angle =ref(1);
% state_servo2.angle =ref(2);
% plot_arm(f,state_servo1,state_servo2)
% 逆運動学で直線を出す
y_ref=-0.05:0.005:0.05;
x_ref=ones(1,length(y_ref)).*0.15;
% 逆運動学で円を出す
% th=linspace(0,2*pi,100);
% R=0.01;
% x_ref=sin(th).*R +0.15;
% y_ref=cos(th).*R +0;
% アニメーション実行
t=0:Ts:(length(x_ref)-1)*Ts;
data_servo1_anlge=0;data_servo2_anlge=0;
data_servo1_anlge_J=0;data_servo2_anlge_J=0;
JacobianTheta=0;last_p=0; %ヤコビ行列で計算するとき
for i=1:length(x_ref)
% 解析解を用いた場合
ref=gen_servo_angle(x_ref(i),y_ref(i));
% ヤコビ行列を用いた場合
if i==1
last_p=[x_ref(1);y_ref(1)];
JacobianTheta=[ref(1);ref(2)];
else
J=JacobianMatrix(state_servo1.angle,state_servo2.angle,...
state_servo1.length,state_servo2.length);
delta_p=[x_ref(i);y_ref(i)]-last_p;
last_p = [x_ref(i);y_ref(i)];
JacobianTheta = JacobianTheta + J\ delta_p;
end
% ヤコビ行列でやる場合は下のコメントアウトを解除
% ref = JacobianTheta';
state_servo1.angle =ref(1);
state_servo2.angle =ref(2);
data_servo1_anlge(i)=ref(1);
data_servo2_anlge(i)=ref(2);
data_servo1_anlge_J(i)=JacobianTheta(1);
data_servo2_anlge_J(i)=JacobianTheta(2);
plot_arm(f,state_servo1,state_servo2)
pause(20/1000);
end
% 角度情報を見てみる
figure();
plot(t,rad2deg(data_servo1_anlge),t,rad2deg(data_servo2_anlge),...
t,rad2deg(data_servo1_anlge_J),t,rad2deg(data_servo2_anlge_J));
legend('servo1','servo2','servo1_J','servo2_J')
% ヤコビ行列を求める
function J=JacobianMatrix(theta1,theta2,L1,L2)
J=[-L1 * sin(theta1)-L2*sin(theta1+theta2) ,-L2*sin(theta1+theta2);
L1*cos(theta1)+L2*cos(theta1+theta2), L2*cos(theta1+theta2)];
end
function plot_arm(f,servo1,servo2)
figure(f);
cla; %この行をコメントアウトするとアームのステップごとの位置が見れる
% arm 1st
plot([0.0 , servo1.length * cos(servo1.angle)],...
[0.0 ,servo1.length * sin(servo1.angle)],'b');
% arm 2nd
plot([servo1.length * cos(servo1.angle) ,...
servo1.length * cos(servo1.angle)...
+ servo2.length * cos(servo1.angle + servo2.angle)],...
[servo1.length * sin(servo1.angle) ,...
servo1.length * sin(servo1.angle)...
+ servo2.length * sin(servo1.angle + servo2.angle)],'r');
end
function theta=gen_servo_angle(x,y)
L1=state_servo1.length;
L2=state_servo2.length;
% ここで+-の候補が出てくる．
theta(1)=atan2(y,x)...
-acos( (x^2 + y^2 + L1^2 - L2^2)/(2*L1 * sqrt(x^2+y^2)) );
% +acos( (x^2 + y^2 + L1^2 - L2^2)/(2*L1 * sqrt(x^2+y^2)) );
theta(2)=-theta(1) + atan2((y-L1*sin(theta(1))) , (x-L1*cos(theta(1))));
end
end
Reference
이 문제에 관하여(2 링크 매니퓰레이터의 궤도 추적 제어), 우리는 이곳에서 더 많은 자료를 발견하고 링크를 클릭하여 보았다 https://qiita.com/fumiya_sato/items/9167f12bf182fa585968텍스트를 자유롭게 공유하거나 복사할 수 있습니다.하지만 이 문서의 URL은 참조 URL로 남겨 두십시오.
우수한 개발자 콘텐츠 발견에 전념
(Collection and Share based on the CC Protocol.)
좋은 웹페이지 즐겨찾기
