6軸機械臂正逆解運算實現
6軸機械臂正逆解運算實現
利用Gluon-6L3機械臂模型的引數（引數來源於知乎@樑政），對機械臂進行運動學分析。
這裡採用標準DH座標系，並將d6設定為0，方便後續計算。
首先，SDH的變換矩陣為：
i
i
−
1
T
=
A
i
=
^{i-1}_iT=A_i=
ii−1T=Ai=
[
c
i
−
s
i
c
α
i
s
i
s
α
i
a
i
c
i
s
i
c
i
c
α
i
−
c
i
s
α
i
a
i
s
i
0
s
α
i
c
α
i
d
i
0
0
0
1
]
\begin{bmatrix} c_i & -s_ic\alpha_i & s_is\alpha_i& a_ic_i \\ s_i & c_ic\alpha_i & -c_is\alpha_i& a_is_i\\ 0 & s\alpha_i & c\alpha_i&d_i\\ 0&0&0&1\end{bmatrix}
⎣⎢⎢⎡cisi00−sicαicicαisαi0sisαi−cisαicαi0aiciaisidi1⎦⎥⎥⎤
我們將機械臂參數列中的資料帶入，得到6個變換矩陣
1 0 T = ^{0}_1T= 10T= [ c 1 0 s 1 0 s 1 0 − c 1 0 0 1 0 d 1 0 0 0 1 ] \begin{bmatrix} c_1& 0 & s_1 &0\\ s_1 & 0 & -c_1 & 0\\ 0 & 1 & 0 &d_1\\ 0 &0 &0 &1 \end{bmatrix} ⎣⎢⎢⎡c1s1000010s1−c10000d11⎦⎥⎥⎤ 2 1 T = ^{1}_2T= 21T= [ c 2 − s 2 0 a 2 c 2 s 2 c 2 0 a 2 s 2 0 0 1 0 0 0 0 1 ] \begin{bmatrix} c_2& -s_2 & 0 &a_2c_2\\ s_2 & c_2 & 0 & a_2s_2\\ 0 & 0 & 1 &0\\ 0 &0 &0 &1 \end{bmatrix} ⎣⎢⎢⎡c2s200−s2c2000010a2c2a2s201⎦⎥⎥⎤
3 2 T = ^{2}_3T= 32T= [ c θ 3 − s θ 3 0 a 3 c θ 3 s θ 3 c θ 3 0 a 3 s θ 3 0 0 1 0 0 0 0 1 ] \begin{bmatrix} c\theta_3& -s\theta_3 & 0 &a_3c\theta_3\\ s\theta_3 & c\theta_3 & 0 & a_3s\theta_3\\ 0 & 0 & 1 &0\\ 0 &0 &0 &1 \end{bmatrix} ⎣⎢⎢⎡cθ3sθ300−sθ3cθ3000010a3cθ3a3sθ301⎦⎥⎥⎤ 4 3 T = ^{3}_4T= 43T= [ c θ 4 0 s θ 4 0 s θ 4 0 − c θ 4 0 0 1 0 d 4 0 0 0 1 ] \begin{bmatrix} c\theta_4& 0 & s\theta_4 &0\\ s\theta_4 & 0 & -c\theta_4 & 0\\ 0 & 1 & 0 &d_4\\ 0 &0 &0 &1 \end{bmatrix} ⎣⎢⎢⎡cθ4sθ4000010sθ4−cθ40000d41⎦⎥⎥⎤
5 4 T = ^{4}_5T= 54T= [ c θ 5 0 − s θ 5 0 s θ 5 0 c θ 5 0 0 − 1 0 d 5 0 0 0 1 ] \begin{bmatrix} c\theta_5& 0 & -s\theta_5 &0\\ s\theta_5 & 0 & c\theta_5 & 0\\ 0 & -1 & 0 &d_5\\ 0 &0 &0 &1 \end{bmatrix} ⎣⎢⎢⎡cθ5sθ50000−10−sθ5cθ50000d51⎦⎥⎥⎤ 6 5 T = ^{5}_6T= 65T= [ c θ 6 − s θ 6 0 0 s θ 6 c θ 6 0 0 0 0 1 0 0 0 0 1 ] \begin{bmatrix} c\theta_6& -s\theta_6 & 0 &0\\ s\theta_6 & c\theta_6 & 0 & 0\\ 0 & 0 & 1 &0\\ 0 &0 &0 &1 \end{bmatrix} ⎣⎢⎢⎡cθ6sθ600−sθ6cθ60000100001⎦⎥⎥⎤
機械臂的位姿矩陣T為：
T
=
1
0
T
2
1
T
3
2
T
4
3
T
5
4
T
6
5
T
=
T=^{0}_1T^{1}_2T^{2}_3T^{3}_4T^{4}_5T^{5}_6T=
T=10T21T32T43T54T65T=
[
n
x
o
x
a
x
p
x
n
y
o
y
a
y
p
y
n
z
o
z
a
z
p
z
0
0
0
1
]
\begin{bmatrix} n_x& o_x & a_x &p_x\\ n_y & o_y& a_y & p_y \\ n_z & o_z & a_z & p_z \\ 0 &0 &0 &1 \end{bmatrix}
⎣⎢⎢⎡nxnynz0oxoyoz0axayaz0pxpypz1⎦⎥⎥⎤
n
x
=
c
1
c
5
c
6
c
234
−
c
1
s
6
s
234
+
s
1
s
5
s
6
n_x=c_1c_5c_6c_{234}-c_1s_6s_{234}+s_1s_5s_6
nx=c1c5c6c234−c1s6s234+s1s5s6
n
y
=
s
1
c
5
c
6
c
234
−
c
1
c
6
c
6
−
s
1
s
5
s
234
n_y=s_1c_5c_6c_{234}-c_1c_6c_{6}-s_1s_5s_{234}
ny=s1c5c6c234−c1c6c6−s1s5s234
n
z
=
c
5
c
6
s
234
+
s
6
c
234
n_z=c_5c_6s_{234}+s_6c_{234}
nz=c5c6s234+s6c234
o
x
=
−
c
1
c
5
s
6
c
234
−
s
1
s
5
s
6
−
c
1
c
6
s
234
o_x=-c_1c_5s_6c_{234}-s_1s_5s_6-c_1c_6s_{234}
ox=−c1c5s6c234−s1s5s6−c1c6s234
o
y
=
−
s
1
c
5
s
6
c
234
+
c
1
c
5
s
6
−
s
1
c
6
s
234
o_y=-s_1c_5s_6c_{234}+c_1c_5s_6-s_1c_6s_{234}
oy=−s1c5s6c234+c1c5s6−s1c6s234
o
z
=
c
6
c
234
−
s
6
c
5
s
234
o_z=c_6c_{234}-s_6c_5s_{234}
oz=c6c234−s6c5s234
a
x
=
c
5
s
1
−
c
1
c
5
c
234
a_x=c_5s_1-c_1c_5c_{234}
ax=c5s1−c1c5c234
a
y
=
−
c
5
c
1
−
s
1
s
5
c
234
a_y=-c_5c_1-s_1s_5c_{234}
ay=−c5c1−s1s5c234
a
z
=
−
s
5
s
234
a_z=-s_5s_{234}
az=−s5s234
p
x
=
c
1
d
5
s
234
+
s
1
d
4
+
a
3
c
1
c
23
+
a
2
c
2
c
1
p_x=c_1d_5s_{234}+s_1d_4+a_3c_1c_{23}+a_2c_2c_1
px=c1d5s234+s1d4+a3c1c23+a2c2c1
p
y
=
s
1
d
5
s
234
−
c
1
d
4
+
a
3
s
1
c
23
+
a
2
c
2
s
1
p_y=s_1d_5s_{234}-c_1d_4+a_3s_1c_{23}+a_2c_2s_1
py=s1d5s234−c1d4+a3s1c23+a2c2s1
p
z
=
−
c
234
d
5
+
a
3
s
23
+
a
2
s
2
+
d
1
p_z=-c_{234}d_5+a_3s_{23}+a_2s_2+d_1
pz=−c234d5+a3s23+a2s2+d1
運動學正解
Euler Angles-由T推算按angles
bata = atan2(sqrt(pow(T.matric[2][0], 2) + pow(T.matric[2][1], 2)), T.matric[2][2]);
alpha = atan2(T.matric[1][2] / sin(bata), T.matric[0][2] / sin(bata));
gama = atan2(T.matric[2][1] / sin(bata), -T.matric[2][0] / sin(bata));
運動學逆解
將末端位姿輸入，反求T中的元素
CaculateThta(double x, double y, double z, double alpha, double bata, double gama)
T
=
[
c
α
c
β
c
γ
−
s
α
s
γ
−
c
α
c
β
s
γ
−
s
α
c
γ
c
α
s
β
p
x
s
α
c
β
c
γ
+
c
α
s
γ
−
s
α
c
β
c
γ
+
c
α
c
γ
s
α
s
β
p
y
−
s
β
c
γ
s
β
s
γ
c
β
p
z
0
0
0
1
]
T=\begin{bmatrix} c_\alpha c_\beta c_\gamma-s_\alpha s_\gamma& -c_\alpha c_\beta s_\gamma-s_\alpha c_\gamma &c_\alpha s_\beta &p_x\\ s_\alpha c_\beta c_\gamma+c_\alpha s_\gamma& -s_\alpha c_\beta c_\gamma+c_\alpha c_\gamma& s_\alpha s_\beta & p_y \\ -s_\beta c_\gamma& s_\beta s_\gamma &c_\beta & p_z \\ 0 &0 &0 &1 \end{bmatrix}
T=⎣⎢⎢⎡cαcβcγ−sαsγsαcβcγ+cαsγ−sβcγ0−cαcβsγ−sαcγ−sαcβcγ+cαcγsβsγ0cαsβsαsβcβ0pxpypz1⎦⎥⎥⎤
首先，我們對矩陣
1
0
T
^{0}_1T
10T進行逆變換
1 0 T − 1 T = 2 1 T 3 2 T 4 3 T 5 4 T 6 5 T = [ c 5 c 6 c 234 − s 6 s 234 − c 5 s 6 c 234 − c 6 s 234 − s 5 c 234 d 5 s 234 + a 3 c 23 + a 2 c 2 c 5 c 6 s 234 − s 6 c 234 − c 5 s 6 s 234 + c 6 c 234 − s 5 s 234 − d 5 c 234 + a 3 s 23 + a 2 s 2 c 6 s 5 − s 6 s 5 c 5 d 4 0 0 0 1 ] = ^{0}_1T^{-1}T=^{1}_2T^{2}_3T^{3}_4T^{4}_5T^{5}_6T=\begin{bmatrix} c_5c_6c_{234}-s_6s_{234}& -c_5s_6c_{234}-c_6s_{234} & -s_5c_{234} &d_5s_{234}+a_3c_{23}+a_2c_2\\ c_5c_6s_{234}-s_6c_{234}& -c_5s_6s_{234}+c_6c_{234} & -s_5s_{234} &-d_5c_{234}+a_3s_{23}+a_2s_2\\ c_6s_5 &-s_6s_5 & c_5 &d_4\\ 0 &0 &0 &1 \end{bmatrix}= 10T−1T=21T32T43T54T65T=⎣⎢⎢⎡c5c6c234−s6s234c5c6s234−s6c234c6s50−c5s6c234−c6s234−c5s6s234+c6c234−s6s50−s5c234−s5s234c50d5s234+a3c23+a2c2−d5c234+a3s23+a2s2d41⎦⎥⎥⎤= [ c 1 n x + s 1 n y c 1 o x + s 1 0 y c 1 a x + s 1 a y c 1 p x + s 1 p y n z o z a z p y − d 1 s 1 n x − c 1 n y s 1 o x − c 1 0 y s 1 a x − c 1 a y s 1 p x − c 1 p y 0 0 0 1 ] \begin{bmatrix} c_1n_x+s_1n_y& c_1o_x+s_10_y & c_1a_x+s_1a_y &c_1p_x+s_1p_y\\ n_z & o_z& a_z & p_y-d_1 \\ s_1n_x-c_1n_y& s_1o_x-c_10_y & s_1a_x-c_1a_y &s_1p_x-c_1p_y\\ 0 &0 &0 &1 \end{bmatrix} ⎣⎢⎢⎡c1nx+s1nynzs1nx−c1ny0c1ox+s10yozs1ox−c10y0c1ax+s1ayazs1ax−c1ay0c1px+s1pypy−d1s1px−c1py1⎦⎥⎥⎤
到這一步某些 θ \theta θ角已經現形了，這裡計算儘量統一位atan2。
計算
θ
1
\theta_1
θ1
d
4
=
s
1
p
x
−
c
1
p
y
d_4=s_1p_x-c_1p_y
d4=s1px−c1py (1)
令
p
x
=
c
o
s
ϕ
,
p
y
=
s
i
n
ϕ
p_x=cos\phi,p_y=sin\phi
px=cosϕ,py=sinϕ帶入(1)得
s
i
n
(
θ
1
−
ϕ
)
=
d
4
sin(\theta_1-\phi)=d_4
sin(θ1−ϕ)=d4
c
o
s
(
θ
1
−
ϕ
)
=
±
1
−
d
4
2
cos(\theta_1-\phi)=\pm\sqrt{1-d_4^2} \quad
cos(θ1−ϕ)=±1−d42
θ
1
=
a
t
a
n
2
(
p
y
,
p
x
)
+
a
t
a
n
2
(
d
4
,
±
1
−
d
4
2
)
\theta_1=atan2(p_y,p_x)+atan2(d_4,\pm\sqrt{1-d_4^2} \quad)
θ1=atan2(py,px)+atan2(d4,±1−d42)
計算
θ
5
\theta_{5}
θ5
已知
c
5
=
s
1
a
x
−
c
1
a
y
c_{5}=s_1a_x-c_1a_y
c5=s1ax−c1ay
s
5
=
±
1
−
c
5
2
s_{5}=\pm\sqrt{1-c_5^2} \quad
s5=±1−c52
θ
5
=
a
t
a
n
2
(
s
5
,
c
5
)
\theta_{5}=atan2(s_{5},c_{5})
θ5=atan2(s5,c5)
計算
θ
6
\theta_6
θ6
c
6
s
5
=
s
1
n
x
−
c
1
n
y
c_6s_5=s_1n_x-c_1n_y
c6s5=s1nx−c1ny
−
s
6
s
5
=
s
1
o
x
−
c
1
o
y
-s_6s_5=s_1o_x-c_1o_y
−s6s5=s1ox−c1oy
θ 6 = a t a n 2 ( ( s 1 n x − c 1 n y ) s 5 , ( s 1 o x − c 1 o y ) − s 5 ) \theta_6=atan2(\frac{(s_1n_x-c_1n_y)}{s_5} \quad,\frac{(s_1o_x-c_1o_y)}{-s_5} \quad) θ6=atan2(s5(s1nx−c1ny),−s5(s1ox−c1oy))
計算
θ
3
\theta_3
θ3
已知
s
234
=
a
z
−
s
5
,
c
234
=
c
1
a
x
+
s
1
a
y
−
s
5
s_{234}=\frac{a_z}{-s_5} \quad,c_{234}=\frac{c_1a_x+s_1a_y}{-s_5} \quad
s234=−s5az,c234=−s5c1ax+s1ay（2）
已知
d
5
s
234
+
a
3
c
23
+
a
2
c
2
=
c
1
p
x
+
s
1
p
y
d_5s_{234}+a_3c_{23}+a_2c_2=c_1p_x+s_1p_y
d5s234+a3c23+a2c2=c1px+s1py（3）
已知
−
d
5
c
234
+
a
3
s
23
+
a
2
s
2
=
p
z
−
d
1
-d_5c_{234}+a_3s_{23}+a_2s_2=p_z-d_1
−d5c234+a3s23+a2s2=pz−d1（4）
令
k
1
=
c
1
p
x
+
s
1
p
y
−
d
5
s
234
,
k
2
=
p
z
−
d
1
+
d
5
c
234
k_1=c_1p_x+s_1p_y-d_5s_{234},k_2=p_z-d_1+d_5c_{234}
k1=c1px+s1py−d5s234,k2=pz−d1+d5c234
c 3 = k 1 2 + k 2 2 − a 2 2 − a 3 3 2 a 2 a 3 c_3=\frac{k_1^2+k_2^2-a_2^2-a_3^3}{2a_2a_3} \quad c3=2a2a3k12+k22−a22−a33
s
3
=
±
1
−
c
3
2
s_3=\pm\sqrt{1-c_3^2} \quad
s3=±1−c32
θ
3
=
a
t
a
n
2
(
s
3
,
c
3
)
\theta_{3}=atan2(s_{3},c_{3})
θ3=atan2(s3,c3)
計算
θ
2
\theta_2
θ2
繼續利用上面三個公式（2）（3）（4）求解
θ
2
\theta_2
θ2
s 2 = ( k 2 ( a 2 + a 3 c 3 ) − k 1 a 3 s 3 ) a 1 2 + a 2 2 + 2 a 1 a 2 c 3 s_2=\frac{(k_2(a_2+a_3c_3)-k_1a_3s_3)}{a_1^2+a_2^2+2a_1a_2c_3} \quad s2=a12+a22+2a1a2c3(k2(a2+a3c3)−k1a3s3)
c 2 = k 2 − s 2 ( a 2 + a 3 c 3 ) a 3 s 3 c_2=\frac{k_2-s_2(a_2+a_3c_3)}{a_3s_3} \quad c2=a3s3k2−s2(a2+a3c3)
θ 2 = a t a n 2 ( s 2 , c 2 ) \theta_{2}=atan2(s_{2},c_{2}) θ2=atan2(s2,c2)
計算
θ
4
\theta_4
θ4
已經到了這地步了，
θ
4
\theta_4
θ4隨意發揮（實在敲不動了），下次繼續
相關文章
- 四軸機械臂控制機械臂
- 機械鍵盤黑軸紅軸青軸茶軸區別 機械鍵盤黑軸紅軸青軸茶軸玩遊戲打字哪個好遊戲
- 使用Azero遠端語音控制機械臂機械臂
- 從虛擬世界伸到現實的機械臂，靠攝像機就能玩轉任何物體虛擬世界機械臂
- 分手後美女設計師造機器人男友：機械臂解吊帶機器人機械臂
- WPF開發經驗-實現一種三軸機械手控制元件控制元件
- 機械鍵盤什麼軸好 機械鍵盤追求手感打字玩遊戲該選什麼軸遊戲
- 機械鍵盤指南（一）縱覽與軸
- 孿生工廠：機械臂加工產線 HMI 監控介面機械臂
- 逆波蘭計算器分析和實現
- 學習靈巧的手部操作，強化學習賦能機械臂強化學習機械臂
- 【計算機組成原理】第6章 計算機的運算方法計算機
- 正則實現數學運算
- 機械臂論文筆記（一）【基於卷積神經網路的二指機械手 抓取姿態生成研究 】機械臂筆記卷積神經網路
- 位運算與SQL實現SQL
- 四則運算實現 (轉)
- javascript怎麼實現算術加法運算JavaScript
- 電子眼、機械臂、5G應用 這樣的“助手”有點強JCP機械臂
- 利用PDM實現機械製造業的資訊整合
- sobel運算元，matlab實現Matlab
- 位運算可以實現哪些功能
- 鍵盤軸色不同功能有什麼區別 機械鍵盤的各種軸有什麼區別
- 使用Actor-Critic的DDPG強化學習演算法控制雙關節機械臂強化學習演算法機械臂
- 使用sklearn實現svm--用於機械故障分類
- Python實現雙X軸雙Y軸繪圖Python繪圖
- 誰說機械臂就一定粗魯？這個機器人你碰一下它就不動機械臂機器人
- 大整數運算C#實現C#
- Vlan間路由實驗（單臂路由實現法）薦路由
- 機械磁碟iops說明及計算方法
- 日本最新研究：從你腦中自動領任務的機械手，三頭六臂不是夢
- Verilog實現加減乘除運算
- 串的基本運算實現-加密解密串加密解密
- awk+uniq實現集合減法運算
- 2，javase程式碼實戰-運算子——位運算實現加密解密 (一)Java加密解密
- android位運算講解與實戰Android
- 計算機基礎：位運算計算機
- 逆波蘭計算器
- C/C++模運算（正負整數）C++