産業用ロボットの運動学とMatlabの順解法・逆解法アルゴリズム研究ノート（ハートフルセッションのまとめ）(2)

---

CSDN が、文字数が多すぎて 1 つの記事を投稿できないため、分割する必要があることを通知しました

だいたい

• スタンダードDHモデル
• 改良されたDHモデル
• ロボットの運動学

詳細については、最初の記事を参照してください。

→→→ [産業用ロボット運動学とMatlabの順解法・逆解法アルゴリズム検討ノート（​​全体セッションの概要）(1)]

❤ 2023.6.19 ❤

ロボットの逆運動学

※代数解、幾何解、解析解（閉解）、数値解の意味とつながり

そういえば、私も初めてこれらの概念に出会ったとき、かなりめまいを感じましたが、他の人とコミュニケーションを取ってみると、めまいを感じていたのは自分だけではなかったことがわかりました。

次に、ChatGPT で最初に回答してください

それでは要約します

代数解も幾何解も解析解（閉じた解）に属し、具体的な方程式を列挙することができ、得られる結果が正確であることが特徴です。違いは、代数的解法は行列の特性に基づいており、等しい特定の要素の方法を通じて方程式系を確立することによって未知数が解決されるのに対し、幾何学的解法は幾何学的関係を通じて方程式系を確立することであるということです。未知を解決するロボット。

特定の構成を持つロボットのみが分析ソリューションを備えていますが、幸いなことに、私たちが通常使用するロボットのほとんどは特定の構成のものです。

数値解は解析解に相当し、演算処理が反復に依存し、得られる結果が近似解となるのが特徴です。特殊な構成と一般的な構成を含むすべてのロボットに適用でき、主に分析ソリューションを持たないロボットで使用されます。

---

ZK-500 ロボットは後部 3 軸が 1 点で交差する特殊な配置に属しているため、ここでは解析手法を使用してロボットの逆解を求めます。

まず、ポーズ記述行列は次のように書かれます

$${}_6^{}T=\left[\begin{array}{cccc}{n}_{}& {ああ}_{}& {ある}_{}& p_{}\\ n_{}& {ああ}_{}& {ある}_{}& p_{}\\ n_{}& {ああ}_{}& {ある}_{}& p_{}\\ & & & \end{array}\right]={}_1^{}T\left({私は}_{}\right){}_2^{}T\left({私は}_{}\right){}_3^{}T\left({私は}_{}\right){}_4^{}T\left({私は}_{}\right){}_5^{}T\left({私は}_{}\right){}_6^{}T\left({私は}_{}\right)$$

△ θ 1 \theta_1の代数解${私}_{}$、${私}_{}$、${私}_{}$

※参考資料

→→→ 【MD-Hモデル運動学の順解・逆解と姿勢角の検証】
これは本当にお宝作者です

→→→ロボット工学入門 (ジョン・J・クレイグ)

---

※なぜatan2()関数を使って解決するのかについて

$アタン2\left(\right)$ 、この関数は A Tan ( ) \mathrm{Atan}\left(\right)と同じです。$日焼け\left(\right)$は逆正接と同じですが、値の範囲は$(-\pi ,\pi )$を使用すると、角度に対応する象限を取得できます。これは、$日焼け\left(\right)$。

詳細は
→→→ atan2()
→→→ atan2 vs atanを参照してください。

なぜこの関数を使うのかというと、この時点では私も非常に混乱していましたが、いくつかのコーナーのsin値またはcos値を計算した後、理論的には、逆正弦/逆余弦関数を使用して対応する角度を計算できます。と cos を同時に計算し、それを分割して atan2() を使用して解決しますか?

情報を調べていたらこんな記事を見つけました

→→→ [ロボット運動学の逆解​​法には、アーク/コサイン関数の代わりに二変数逆正接関数 atan2 を使用する方が良いのはなぜですか? 】

要約すると、一般的な意味は、アークサイン/アークコサイン関数を直接使用することは不可能ではないが、その値の範囲が比較的狭く、ロボットの関節範囲が( − π , π )に達する可能性があるということです。 (-\pi,\pi $(-\pi ,\pi )$を逆正弦関数/逆余弦関数を使用して解く場合は、さまざまなポーズの値の範囲を十分に考慮して判断する必要があります。また、atan2() 関数の値の範囲は$(-\pi ,\pi )$の場合、余分な判定処理が省略される場合が多い。

また、arcsin()/arccos() 関数にはいくつかの特別な点で定義が欠けており、計算結果の誤差が増幅されますが、atan2() 関数にはこの問題はありません。

atan2() 関数の定義

• atan2() 関数使用時に注意が必要な問題

しかし、記事によると[ロボット運動学の逆解​​法には、アーク/コサイン関数の代わりに 2 変数アークタンジェント関数 atan2 を使用するのが最適な理由は何ですか?] 】前述したように、atan2() を使用した後は確実ではなく、いくつかの問題に注意する必要があります。
1. x=0, y=0 のときの定義
2. x または y のときの関数の値のジャンプ0を通過する

具体的な解決策については上記の記事を参照してください

---

○ θ 1 \theta_1を解く${私}_{}$

まず、上の式に${私}_{}$方程式の左側へのシフトの一部

$${\left[{}_1^{}T\left({私は}_{}\right)\right]}^{-1}{}_6^{}T={}_2^{}T\left({私は}_{}\right){}_3^{}T\left({私は}_{}\right){}_4^{}T\left({私は}_{}\right){}_5^{}T\left({私は}_{}\right){}_6^{}T\left({私は}_{}\right)$$

1 0 T {_1^0} T${}_1^{}T$の逆数を求めるには、方程式は次のように書かれます。

$$\left[\begin{array}{cccc}{c}_{}& s_{}& 0& 0\\ -s_{}& c_{}& 0& 0\\ 0& 0& 1& -d_{}\\ & & & \end{array}\right]\left[\begin{array}{cccc}{n}_{}& {ああ}_{}& {ある}_{}& p_{}\\ n_{}& {ああ}_{}& {ある}_{}& p_{}\\ n_{}& {ああ}_{}& {ある}_{}& p_{}\\ & & & \end{array}\right]={}_6^{}T$$

6 1 T {_6^1}Tを解く${}_6^{}T$の後、方程式の両側の行列を観察して、要素 (2,4) を等しくすると、次の結果が得られます。

$$-s_{}{p}_{}+c_{}{p}_{}=0$$

---

• ※ 【この式の求め方】

この解法プロセスは内容が多いので、matlabを使って計算していきます。。。

• 方程式の左側の行列 (各角括弧は行を表します)

[nx*cos(Q1) + ny*sin(Q1), 
 ox*cos(Q1) + oy*sin(Q1), 
 ax*cos(Q1) + ay*sin(Q1), 
 px*cos(Q1) + py*sin(Q1)]
 
[ny*cos(Q1) - nx*sin(Q1), 
 oy*cos(Q1) - ox*sin(Q1), 
 ay*cos(Q1) - ax*sin(Q1), 
 py*cos(Q1) - px*sin(Q1)]
 
[nz,oz,az,pz - d1]

[0,0,0,1]

• 方程式の右側の行列 (各角括弧は行を表します)

[- cos(Q6)*(sin(Q2 + Q3)*sin(Q5) - cos(Q2 + Q3)*cos(Q4)*cos(Q5)) - cos(Q2 + Q3)*sin(Q4)*sin(Q6),   
 sin(Q6)*(sin(Q2 + Q3)*sin(Q5) - cos(Q2 + Q3)*cos(Q4)*cos(Q5)) - cos(Q2 + Q3)*cos(Q6)*sin(Q4), 
 sin(Q2 + Q3)*cos(Q5) + cos(Q2 + Q3)*cos(Q4)*sin(Q5), 
 a1 + a3*cos(Q2 + Q3) + d4*sin(Q2 + Q3) + a2*cos(Q2)]
 
[- cos(Q4)*sin(Q6) - cos(Q5)*cos(Q6)*sin(Q4),
 cos(Q5)*sin(Q4)*sin(Q6) - cos(Q4)*cos(Q6),
 sin(Q4)*sin(Q5),
 0]
 
[cos(Q6)*(cos(Q2 + Q3)*sin(Q5) + sin(Q2 + Q3)*cos(Q4)*cos(Q5)) - sin(Q2 + Q3)*sin(Q4)*sin(Q6), 
 - sin(Q6)*(cos(Q2 + Q3)*sin(Q5) + sin(Q2 + Q3)*cos(Q4)*cos(Q5)) - sin(Q2 + Q3)*cos(Q6)*sin(Q4), 
 sin(Q2 + Q3)*cos(Q4)*sin(Q5) - cos(Q2 + Q3)*cos(Q5),      
 a3*sin(Q2 + Q3) - d4*cos(Q2 + Q3) + a2*sin(Q2)]
 
[0,0,0,1]
 

案の定、${私}_{}$(ここでは$Q_{}$) 要素 (2,4) のうちの が最良の計算です。。。

---

式では、$p_{}$、$p_{}$が既知の場合、${私}_{}$次のように表現できます。

$${私}_{}=アタン2\left(p_{}、p_{}\right)$$

入力変数は最初に$p_{}$次に$p_{}$Tanの場合⁡ θ = pypx \tan{\theta}=\frac{p_y}{p_x
}$\mathrm{黄褐色}私=\frac{p_{}}{p_{}}$つまり、最初に分子を入力し、次に分母を入力します。

ここには 2 つの解セットがあります:
$${私}_{}=アタン2\left(p_{}、p_{}\right)$$
または$${私}_1^{}=アタン2\left(-p_{}、-p{\_}_{}\right)$$

---

• Matlab コードの実装

%% theta1求解
%theta1可能的解有两组
theta1_1=atan2(py,px);
theta1_2=atan2(-py,-px);

---

○ θ 3 \theta_3を解く${私}_{}$

上の方程式の行列要素 (1,4) と (3,4) が次と等しいとします。

$$\begin{gathered} c_{}{p}_{}+s_{}{p}_{}={ある}_{}+{ある}_{}{c}_{}+{ある}_{}{c}_{}+d_{}{s}_{} \\ {p}_{}-d_{}={ある}_{}{s}_{}+{ある}_{}{s}_{}-d_{}{c}_{} \end{gathered}$$

等号の周りの二乗の合計は
$${メートル}^2+n^2={ある}_2^{}+{ある}_3^{}+d_4^{}+2a\_{\_}_{}\left({\_}_{}{c}_{}+d_{}{s}_{}\right)$$
そのうち:
$$\begin{gathered} メートル=c_{}{p}_{}+s_{}{p}_{}-{ある}_{} \\ n=p_{}-d_{} \end{gathered}$$

---

このステップの計算プロセスについて:

このコードを使って説明します。

syms a1 a2 a3 d4 theta2 theta3

m=a2*cos(theta2)+a3*cos(theta2+theta3)+d4*sin(theta2+theta3);
n=a2*sin(theta2)+a3*sin(theta2+theta3)-d4*cos(theta2+theta3);

k=m^2 + n^2

% 展开表达式为多项式形式
K1 = expand(m^2 + n^2) 
%K = collect(K, [a1 a2 a3 d4]);  

% 化简多项式
K2=simplify(K1)

% 整理多项式的项
k3=collect(K2)

結果：

k =
(a3*cos(theta2 + theta3) + d4*sin(theta2 + theta3) + a2*cos(theta2))^2 + (a3*sin(theta2 + theta3) - d4*cos(theta2 + theta3) + a2*sin(theta2))^2
 
K1 =
a2^2*cos(theta2)^2 + a2^2*sin(theta2)^2 + a3^2*cos(theta2)^2*cos(theta3)^2 + d4^2*cos(theta2)^2*cos(theta3)^2 + a3^2*cos(theta2)^2*sin(theta3)^2 + a3^2*cos(theta3)^2*sin(theta2)^2 + d4^2*cos(theta2)^2*sin(theta3)^2 + d4^2*cos(theta3)^2*sin(theta2)^2 + a3^2*sin(theta2)^2*sin(theta3)^2 + d4^2*sin(theta2)^2*sin(theta3)^2 + 2*a2*a3*cos(theta2)^2*cos(theta3) + 2*a2*a3*cos(theta3)*sin(theta2)^2 + 2*a2*d4*cos(theta2)^2*sin(theta3) + 2*a2*d4*sin(theta2)^2*sin(theta3)
 
K2 =
a2^2 + 2*cos(theta3)*a2*a3 + 2*sin(theta3)*a2*d4 + a3^2 + d4^2
 
k3 =
a2^2 + 2*cos(theta3)*a2*a3 + 2*sin(theta3)*a2*d4 + a3^2 + d4^2

---

令$H=\left(m^2+n^2-{ある}_2^{}-{ある}_3^{}-d_4^{}\right)/{2a}_{}$
この場合、方程式は次のように表すことができます:
$${ある}_{}{c}_{}+d_{}{s}_{}=H$$

得られる結果：
$${私}_{}=アタン2\left(H、\pm \sqrt{{ある}_3^{}+d_4^{}-H^2}\right)-アタン2\left({\_}_{}、d_{}\right)$$

正負の符号が存在するため、${私}_{}$解決策には 2 つのセットがあります。

---

• ※ 【このステップに至った経緯について】

逆正接への変換の具体的な内容は見つかりませんでしたが、この導出過程は以下を参照できます。

→→→ [acosx+bsinx = c; xの計算方法]

---

• Matlab コードの実装

%% theta3求解
%theta1的取值已经确定，但是因为theta1有两组可能得解，所以这里实际上m有两组取值，这里只给出一组
m=cos(theta1)*px+sin(theta1)*py-a1; 
n=pz-d1;
H=(m^2+n^2-a2^2-a3^2-d4^2)/(2*a2);
theta3_1=atan2(H,sqrt(a3^2+d4^2-H^2))-atan2(a3,d4);
theta3_2=atan2(H,-sqrt(a3^2+d4^2-H^2))-atan2(a3,d4);

---

○ θ 2 \theta_2を解く${私}_{}$

\theta_2$の解決策について、いくつかの方法を試してみました。

・ロボティクス入門の方法（失敗例）

まず、教科書『ロボット工学入門』（John J Craig）に記載されている方法を試してみました。

ご存知のとおり、PUMA560は「ロボット工学入門」の事例として使われており、現在一般的に使われている産業用ロボットの構成とは構造が異なる（主に私が研究しているロボットとは異なる）ため、そのまま使用することはできません。 . . 以下にその手順を踏まえた計算を示します。

最初のステップは、ロボットのエンドポーズ記述行列の計算式を次のように整理することです。

$$[{}_3^{}T\left({私は}_{}\right){]}^{-1}{}_6^{}T={}_4^{}T\left({私は}_{}\right){}_5^{}T\left({私は}_{}\right){}_6^{}T\left({私は}_{}\right)$$

電圧の範囲 (1.4) と (2.4) を考慮すると、量
$$\begin{gathered} (p_{}-d_{})罪（{私}_{})-{ある}_{}\cos ({私}_{})-{ある}_{}\cos ({私}_{})+... \\ {p}_{}\cos ({私}_{}\left)\cos \right({私}_{})+p_{}罪（{私}_{})\cos \left({私}_{}\right)={ある}_{} \\ (p_{}-d_{})\cos \left({私}_{}\right)+{ある}_{}罪（{私}_{})+{ある}_{}罪（{私}_{})-... \\ {p}_{}\cos \left({私}_{}\right)\cos \left({私}_{}\right)+p_{}罪（{私}_{}\left)\cos \right({私}_{})=-d_{} \end{gathered}$$

---

• 計算プロセスは次のとおりです

計算プロセス

syms Q1 Q2 Q3 Q4 Q5 Q6  d1 d4 dt a1 a2 a3  nx ny nz ox oy oz ax ay az px py pz

%ZK-500连杆间齐次变换矩阵
T_01 =[ cos(Q1),   -sin(Q1),    0,      0
        sin(Q1),    cos(Q1),    0,      0
        0,          0,          1,      d1
        0,          0,          0,      1];
T_12 =[ cos(Q2),   -sin(Q2),    0,      a1
        0,          0,         -1,      0
        sin(Q2),    cos(Q2),    0,      0
        0,          0,          0,      1];
T_23 =[ cos(Q3),   -sin(Q3),    0,      a2
        sin(Q3),    cos(Q3),    0,      0
        0,          0,          1,      0
        0,          0,          0,      1];
T_34 =[ cos(Q4),   -sin(Q4),    0,      a3
        0,          0,         -1,     -d4
        sin(Q4),    cos(Q4),    0,      0
        0,          0,          0,      1];
T_45 =[ cos(Q5),   -sin(Q5),    0,      0
        0,          0,          1,      0
       -sin(Q5),   -cos(Q5),    0,      0
        0,          0,          0,      1];
T_56 =[ cos(Q6),   -sin(Q6),    0,      0
        0,          0,         -1,      0
        sin(Q6),    cos(Q6),    0,      0
        0,          0,          0,      1];
T_6t=[  1           0           0       0
        0           1           0       0
        0           0           1       dt
        0           0           0       1];

% 计算T_06和T_16的逆矩阵
T_06=[nx ox ax px;ny oy ay py;nz oz az pz;0 0 0 1];
T_03=T_01*T_12*T_23;
T_36=T_34*T_45*T_56;


% 计算T_01的逆矩阵
T_03_inv = inv(T_03);  

%输出结果并化简
T_left=T_03_inv*T_06;
disp("等式左边矩阵：")
%T_left=simplify(T_left)
T_left
disp("等式右边矩阵：")
simplify(T_36)

結果は次のとおりです

T_left =
 
[nz*cos(Q2)*sin(Q3) + nz*cos(Q3)*sin(Q2) - nx*cos(Q1)*sin(Q2)*sin(Q3) - ny*sin(Q1)*sin(Q2)*sin(Q3) + nx*cos(Q1)*cos(Q2)*cos(Q3) + ny*cos(Q2)*cos(Q3)*sin(Q1),

 oz*cos(Q2)*sin(Q3) + oz*cos(Q3)*sin(Q2) - ox*cos(Q1)*sin(Q2)*sin(Q3) - oy*sin(Q1)*sin(Q2)*sin(Q3) + ox*cos(Q1)*cos(Q2)*cos(Q3) + oy*cos(Q2)*cos(Q3)*sin(Q1), 

az*cos(Q2)*sin(Q3) + az*cos(Q3)*sin(Q2) - ay*sin(Q1)*sin(Q2)*sin(Q3) + ax*cos(Q1)*cos(Q2)*cos(Q3) + ay*cos(Q2)*cos(Q3)*sin(Q1) - ax*cos(Q1)*sin(Q2)*sin(Q3), 

pz*sin(Q2 + Q3) - d1*sin(Q2 + Q3) - a1*cos(Q2 + Q3) - a2*cos(Q3) + (px*cos(Q1 + Q2 + Q3))/2 + (py*sin(Q1 + Q2 + Q3))/2 + (px*cos(Q2 - Q1 + Q3))/2 - (py*sin(Q2 - Q1 + Q3))/2]

[nz*cos(Q2)*cos(Q3) - nz*sin(Q2)*sin(Q3) - ny*cos(Q2)*sin(Q1)*sin(Q3) - ny*cos(Q3)*sin(Q1)*sin(Q2) - nx*cos(Q1)*cos(Q2)*sin(Q3) - nx*cos(Q1)*cos(Q3)*sin(Q2),

 oz*cos(Q2)*cos(Q3) - oz*sin(Q2)*sin(Q3) - oy*cos(Q2)*sin(Q1)*sin(Q3) - oy*cos(Q3)*sin(Q1)*sin(Q2) - ox*cos(Q1)*cos(Q2)*sin(Q3) - ox*cos(Q1)*cos(Q3)*sin(Q2),

 az*cos(Q2)*cos(Q3) - az*sin(Q2)*sin(Q3) - ax*cos(Q1)*cos(Q2)*sin(Q3) - ax*cos(Q1)*cos(Q3)*sin(Q2) - ay*cos(Q2)*sin(Q1)*sin(Q3) - ay*cos(Q3)*sin(Q1)*sin(Q2), 

pz*cos(Q2 + Q3) - d1*cos(Q2 + Q3) + a1*sin(Q2 + Q3) + a2*sin(Q3) + (py*cos(Q1 + Q2 + Q3))/2 - (px*sin(Q1 + Q2 + Q3))/2 - (py*cos(Q2 - Q1 + Q3))/2 - (px*sin(Q2 - Q1 + Q3))/2]

[
nx*sin(Q1) - ny*cos(Q1),
ox*sin(Q1) - oy*cos(Q1),
ax*sin(Q1) - ay*cos(Q1),
px*sin(Q1) - py*cos(Q1)
]

[0,0,0, 1]
 
等式右边矩阵：
[
cos(Q4)*cos(Q5)*cos(Q6) - sin(Q4)*sin(Q6),
- cos(Q6)*sin(Q4) - cos(Q4)*cos(Q5)*sin(Q6), 
cos(Q4)*sin(Q5),  
a3
]


[                          
cos(Q6)*sin(Q5),                            
-sin(Q5)*sin(Q6),        
-cos(Q5), 
-d4]

[cos(Q4)*sin(Q6) + cos(Q5)*cos(Q6)*sin(Q4),   
cos(Q4)*cos(Q6) - cos(Q5)*sin(Q4)*sin(Q6), 
sin(Q4)*sin(Q5),   
0]
[0, 0, 0, 1]

次に、ChatGPT を使ってそれを簡略化してみましょう

最終結果はこれです

(pz - d1)*sin(Q23) - a1*cos(Q23) - a2*cos(Q3) + px*cos(Q1)*cos(Q23) + py*sin(Q1)*cos(Q23)=a3;
(pz - d1)*cos(Q23) + a1*sin(Q23) + a2*sin(Q3) - py*cos(Q1)*cos(Q23) + px*sin(Q1)*cos(Q23)=-d4;

---

現時点では、方程式中で$s_{}$そして$c_{}$、それを未知として設定すると、(理論的には) ${私}_{}+{私}_{}$

m 1 = c 1 px + s 1 py − a 1 m 2 = pz − d 1 m 3 = a 3 + a 2 c 3 m_1=c_1p_x+s_1p_y-a_1\\ m_2=p_z-d_1\\ m_3= a_3
$$\begin{gathered} {メートル}_{}=c_{}{p}_{}+s_{}{p}_{}-{ある}_{} \\ {メートル}_{}=p_{}-d_{} \\ {メートル}_{}={ある}_{}+{ある}_{}{c}_{} \end{gathered}$$

$$\begin{gathered} n_{}=-c_{}{p}_{}-s_{}{p}_{}-d_{} \\ {n}_{}={ある}_{} \\ {n}_{}=-d_{}-{ある}_{}{s}_{} \end{gathered}$$

方程式系を
$$\begin{gathered} {メートル}_{}{c}_{}+{メートル}_{}{s}_{}={メートル}_{} \\ {n}_{}{c}_{}+n_{}{s}_{}=n_{} \end{gathered}$$

ソリューションは次のことを行う必要があります

$$\begin{gathered} バツ=s_{}=(m_{}{n}_{}-{メートル}_{}{n}_{})/({メートル}_{}{n}_{}-{メートル}_{}{n}_{}) \\ y=c_{}=-（m_{}{n}_{}-{メートル}_{}{n}_{})/{メートル}_{}{n}_{}-{メートル}_{}{n}_{}) \end{gathered}$$

レベル 23 = 2 (y, x) の場合 \theta_{23}=\mathrm{For2}(y,x )
$${私}_{}=Atan2(y,\times ）$$

次に、 θ 3 \theta_3に従って、${私}_{}$θ 2 \theta_2の値${私}_{}$

【！！? ? 】見た目は問題ないのですが、計算結果が設定値と異なっており、どこに問題があるのか​​分かりませんので、ご覧になれる方がいらっしゃいましたら、ご指摘いただければと思います。

---

• Matlab コードの実装

m1=cos(theta1)*px+sin(theta1)*py-a1;
m2=pz-d1;
m3=a3+a2*cos(theta3);

n1=-cos(theta1)*py-sin(theta1)*px+pz-d1;
n2=a1;
n3=-d4-a2*sin(theta1);

x = (m1*n3 - m3*n1)/(m1*n2 - m2*n1);
y = -(m2*n3 - m3*n2)/(m1*n2 - m2*n1);

theta23=atan2(y,x)-pi/2;%这里减pi/2因为在DH参数中theta2补偿了pi/2

---

· 参照記事内のメソッド (失敗した試行)

先ほど紹介した記事がそれです。

→→→ 【MD-Hモデル運動学の順解・逆解と姿勢角計算の検証】

この記事では、著者は具体的な導出プロセスについては説明せず、MATLAB コードのみを説明しました。コードは次のとおりです。

%theta2
c3=cosd(Theta3);s3=sind(Theta3);
g1=f2-d6*f1;
g2=f3-d6*az;
g3=a4*c3-d4*s3+a3;
Theta2=(atan2(g3,sqrt(g1^2+g2^2-g3^2))-atan2(g2,g1))*180/pi;
————————————————
版权声明：本文为CSDN博主「Vittore-Li」的原创文章，遵循CC 4.0 BY-SA版权协议，转载请附上原文出处链接及本声明。
原文链接：https://blog.csdn.net/Vittore_Li/article/details/123185721

【！] この記事の著者の DH パラメータの定義は私のものとは異なることに注意してください。私の a1=彼の a2、私の a2=彼の a3、私の a3=彼の a4、および彼の DH モデルの接続ロッドは私の方向と反対です。それが解決策にどのような影響を与えるかわかりません。

長い間眺めていましたが、これがどのようにして起こるのか理解できませんでした。また、直接実行した結果も正しくありませんでした (DH モデルに対応する変更を加えた後に実行しました)。

作者がどのような方法を使用しているか知っている人がいたら、教えていただければ幸いです。（拳を握り締める）

---

・修士論文の方法（正しい結果が得られる方法）

そう、先ほどの上司の論文（前回記事）なのですが、実はこれまでの私のプロセスは基本的に（あるいは全部）上司の手順に沿って書いているのですが、そこに私の理解と検証のプロセスを加えてみました。

上司も違いますし、やり方も今まで見たことのないものです。。。

ロボットの終了位置（座標系 6 の原点）は以下のとおりです。

$$\begin{gathered} p_{}={ある}_{}{c}_{}+{ある}_{}{c}_{}{c}_{}+d_{}{c}_{}{s}_{}+{ある}_{}{c}_{}{c}_{} \\ {p}_{}={ある}_{}{s}_{}+{ある}_{}{s}_{}{c}_{}+d_{}{s}_{}{s}_{}+{ある}_{}{c}_{}{s}_{} \\ {p}_{}=d_{}+{ある}_{}{s}_{}+{ある}_{}{s}_{}-d_{}{c}_{} \end{gathered}$$

取得するために組織されたもの:

$$\begin{gathered} \frac{p_{}}{c_{}}-{ある}_{}={ある}_{}{c}_{}+d_{}{s}_{}+{ある}_{}{c}_{} \\ \frac{p_{}}{s_{}}-{ある}_{}={ある}_{}{c}_{}+d_{}{s}_{}+{ある}_{}{c}_{} \\ {p}_{}-d_{}={ある}_{}{s}_{}+{ある}_{}{s}_{}-d_{}{c}_{} \end{gathered}$$

覚えて

$$k_{}=\frac{p_{}}{c_{}}-{ある}_{}{k_{}=p}_{}-d_{}$$

得る：

$$\begin{gathered} k_{}-{ある}_{}{c}_{}={ある}_{}{c}_{}+d_{}{s}_{} \\ {k}_{}-{ある}_{}{s}_{}={ある}_{}{s}_{}-d_{}{c}_{} \end{gathered}$$

平方和を求めます。

$${ある}_3^{}+d_4^{}=k_1^{}+k_2^{}+{ある}_2^{}-2k{\_}_{}{ある}_{}{c}_{}-2k{\_}_{}{ある}_{}{s}_{}$$

覚えて

$$k_{}=\left(k_1^{}+k_2^{}+{ある}_2^{}-{ある}_3^{}-d_4^{}\right)/{2a}_{}$$

しかし：

$$k_{}{c}_{}+k_{}{s}_{}=k_{}$$
解:
$${私}_{}=アタン2\left(k_{}、\pm \sqrt{k_1^{}+k_2^{}-k_3^{}}\right)-アタン2\left(k_{}、k_{}\right)$$

---

• Matlab コードの実装

k1=px/cos(theta1)-a1;
k2=pz-d1;
k3=(k1^2+k2^2+a2^2-a3^2-d4^2)/(2*a2);

theta2_1=atan2(k3,sqrt(k1^2+k2^2-k3^2))-atan2(k1,k2);
theta2_2=atan2(k3,-sqrt(k1^2+k2^2-k3^2))-atan2(k1,k2);

θ 2 \theta_2を引くことを忘れないでください。${私}_{}$pi/2 のオフセット

---

❤ 2023.6.21 ❤

上記は代数解法を用いて最初の3軸の角度を求めるもので、一般的な手順ではこの時点で最後の4軸を求めることになるのですが、まとめ手法の記事としてはこれだけでは満足できませんの実装方法を 1 つ記録します。

弟の方法を学んだ後は、最初の 3 つの軸を解くのにセット法を使用する方が簡単なはずです。少なくとも頭の体操と同じ代数法よりは理解しやすいと思います。

△ θ 1 \theta_1の幾何解${私}_{}$、${私}_{}$、${私}_{}$

○ θ 1 \theta_1を解く${私}_{}$

ロボットの概略図から、このロボットの構成では、ロッド 1、ロッド 2、およびロッド 3 で構成される機構が同じ垂直面にあり、ロボットの端が共通の原点であることがわかります。ロッド 4、ロッド 5、ロッド 6 の座標系。このことから、既知のロボットの終了座標( px , py , pz ) \left(p_x,p_y,p_z\right) であることがわかります。$\left(p_{}、p_{}、p_{}\right)$軸 1 の${私}_{}$

$${私}_{}=アタン2\left(p_{}、p_{}\right)$$

複数の解決策がある場合さえ考慮しない

---

○ θ 3 \theta_3を解く${私}_{}$

ロボットの構造の概略図は以下のとおりです

図に示すように、点 C' の座標$\left(p_{}、p_{}、p_{}\right)$ ,A 比$\left({\_}_{}\mathrm{コス}{私}_{}、{ある}_{}罪{私}_{}、d_{}\right)$

$$\begin{gathered} L_{B^{「}{C」}^{}}=L_{}=\sqrt{BD{\_}^2+C{D}^2}=\sqrt{{ある}_3^{}+d_4^{}} \\ {L}_{ＡＣ}=\sqrt{ＡＤ{\_}^2-C{D}^2}=\sqrt{{\left({\_}_{}+{ある}_{}\right)}^2-d_4^{}} \\ {L}_{AB{\_}^{}}=L_{ＡＢ}={ある}_{} \end{gathered}$$

点 O、A、C' は同一平面上にあるため、
$$L_{ＡＣ{\_}^{}}=\sqrt{{\left(\sqrt{p_{バツ}^{}+p_y^{}}-{ある}_{}\right)}^2+{\left(p_{}-d_{}\right)}^2}$$

コサインの法則によると、
$$c^2={ある}^2+b^2-2腹筋\mathrm{コス}\gamma$$
得、在$\Delta AB\_{\_}^{{}^{「}}{C」}^{{}^{\text{'}}}$中：
$$\mathrm{コス}{私}_3^{}=\frac{L_{AB{\_}^{』}}^{}+L_{B^{「}{C」}^{』}}^{}-L_{ＡＣ{\_}^{』}}^{}}{2L{\_}_{AB{\_}^{}}{L}_{B^{「}{C」}^{}}}$$

Δ ABC \Delta ABCの余弦の法則からも得られます。$\Delta ABC$中：
$$\mathrm{コス}{私}_{}=\frac{L_{AB\_}^{}+L_{紀元前}^{}-L_{ＡＣ\_}^{}}{2L{\_}_{ＡＢ}{L}_{}}$$
幾何学的関係によると、
$${私}_{}=\mathrm{アークコス}{私}_3^{}-\mathrm{アークコス}{私}_{}$$
コサインの法則の性質に従って、得られた${私}_3^{}$の値は常に 180° 未満であり、${私}_{}+{私}_{}$180°を超える場合もあるので、${私}_{}$値には 2 つのグループがあります。

---

○ θ 2 \theta_2を解く${私}_{}$

θ 2 \theta_2によると${私}_{}$与${私}_{}$異なる値、${私}_{}$この解決策には次の 4 つの状況があります。

$$\begin{gathered} {私}_{}=\angle BAC\_{\_}^{{}^{』}}-\angle B{\_}^{{}^{「}}{AC」}^{{}^{』}} \\ {私}_{}=\angle BAC\_{\_}^{{}^{』}}+\angle B{\_}^{{}^{「}}{AC」}^{{}^{』}} \\ {私}_{}=-\angle B{C}^{{}^{』}}-\angle B{\_}^{{}^{「}}{AC」}^{{}^{』}} \\ {私}_{}=-\angle B{C}^{{}^{』}}+\angle B{\_}^{{}^{「}}{AC」}^{{}^{』}} \end{gathered}$$

図によると、点Bの座標$({\_}_{}\mathrm{コス}{私}_{}、{ある}_{}罪{私}_{}、d_{}+{ある}_{}）$。
点 O、A、B、および C' は同一平面上にあるので、

$$L_{BC{\_}^{}}=\sqrt{{\left(\sqrt{p_{バツ}^{}+p_y^{}}-{ある}_{}\right)}^2+{\left(p_{}-d_{}-{ある}_{}\right)}^2}$$

任$\angle BAC\_{\_}^{{}^{』}}=\beta$，$\angle B{\_}^{{}^{「}}{AC」}^{{}^{』}}=c$

コサインの法則によると、$ABC\_{\_}^{{}^{\text{'}}}$で:

$$\mathrm{コス}b=\frac{L_{AB\_}^{}+L_{ＡＣ{\_}^{』}}^{}-L_{BC{\_}^{』}}^{}}{2L{\_}_{ＡＢ}{L}_{ＡＣ{\_}^{}}}$$

在$\Delta AB{\_}^{{}^{「}}{C」}^{{}^{\text{'}}}$中：
$$\mathrm{コス}c=\frac{L_{AB{\_}^{』}}^{}+L_{ＡＣ{\_}^{』}}^{}-L_{B^{「}{C」}^{』}}^{}}{2L{\_}_{AB{\_}^{}}{L}_{ＡＣ{\_}^{}}}$$

比率:
$${私}_{}=\pm \mathrm{アークコス}b\pm \mathrm{アークコス}\gamma$$
${私}_{}$4 組の解が得られます。

---

○幾何学的手法の概要

幾何学的な方法で最初の 3 つの軸の角度を計算する方法を整理した後、幾何学的な方法はそれほど単純ではないと感じましたが、直感は肯定的ですが、幾何学的な解決策は複数の代数的解決策の状況を改善しません。そして、この多重解は、幾何学的関係による判断が必ずしも代数解より単純であるとは限らないことに基づく必要があります。

計算処理は複雑ではないので、ここでは matlab のコードは記載しません（主に入力しませんでした）。

プロセスに問題がある場合、またはより良い方法がある場合は、お知らせください。（拳を握り締める）

---

△ θ 4 \theta_4の代数解${私}_{}$、${私}_{}$、${私}_{}$

CSDN も、文字数が多すぎて 1 つの記事を投稿できないことを思い出させてくれたので、逆アセンブルして逆運動学θ 4 \theta_4で解く必要がありました。${私}_{}$、${私}_{}$、${私}_{}$、ヤコビ関連コンテンツを参照

→→→ [産業用ロボット運動学とMatlabの順アルゴリズム・逆アルゴリズム学習ノート（全体セッションの概要）(3)]