UnitySir - [Unity模擬] 逆運動學

阿新 • • 發佈：2020-12-22

視訊：

專案地址：

unitysir/6DOF-robot-arm-IK: I built the Unity3d project with meuse.co.jp code. (github.com)

Final IK

單臂機器人逆運動學解析方法

逆運動學解析正文：

解釋機器人手臂逆運動學的解析方法。此方法僅適用於具有球形手腕（關節P4，P5，P6的軸在一個點處交匯）的機器人。並且機器人不應有關節偏移。(Explaining analytical approach to inverse kinematics for robot arm. This method is applyed only for robots that have a spherical wrist (axis of joints P4,P5,P6 meets at one point). And robot should not have joint offsets.)

參考：Hirose的教科書“ Robotics”

向量(a，b) 表示末端執行器的方向和旋轉。(Vector (a,b) represents the direction and rotation of end effector.)

\[(a,b) = R^Y R^P R^R \left[ \matrix{ 1 & 0\\ 0 & 0\\ 0 & 1\\ } \right] \\ \left[ \matrix{ a_x & b_x\\ a_y & b_y\\ a_z & b_z\\ } \right] = R^Y R^P R^R \left[ \matrix{ C_Y C_P & C_Y S_P C_R + S_Y S_R\\ S_Y C_P & S_Y S_P C_R - C_Y S_R\\ -S_P & C_P C_R\\ } \right] \]

偏航角，俯仰角，滾動角由單位滑塊指定。(Yaw,Pitch,Roll angles are given by unity sliders.)

px = S_Slider.value; //position
py = L_Slider.value;
pz = U_Slider.value;
rx = R_Slider.value; //rotation
ry = B_Slider.value;
rz = T_Slider.value;

ax = Mathf.Cos(rz * 3.14f / 180.0f) * Mathf.Cos(ry * 3.14f / 180.0f);
ay = Mathf.Sin(rz * 3.14f / 180.0f) * Mathf.Cos(ry * 3.14f / 180.0f);
az = -Mathf.Sin(ry * 3.14f / 180.0f);

bx = Mathf.Cos(rx * 3.14f / 180.0f) * Mathf.Sin(ry * 3.14f / 180.0f) * Mathf.Cos(rz * 3.14f / 180.0f)
            - Mathf.Sin(rx * 3.14f / 180.0f) * Mathf.Sin(rz * 3.14f / 180.0f);
by = Mathf.Cos(rx * 3.14f / 180.0f) * Mathf.Sin(ry * 3.14f / 180.0f) * Mathf.Sin(rz * 3.14f / 180.0f)
            - Mathf.Sin(rx * 3.14f / 180.0f) * Mathf.Cos(rz * 3.14f / 180.0f);
bz = Mathf.Cos(rx * 3.14f / 180.0f) * Mathf.Cos(ry * 3.14f / 180.0f);

點P₅在向量上與P_r的直線上。 (Point P₅ is on the vector a line from P_r)

\[P_5 = P_r - (l_5 + l_6)a \]

或者 (or)

\[P_{5X} = P_x - (l_5 + l_6)a_x P_{5Y} = P_y - (l_5 + l_6)a_y P_{5Z} = P_z - (l_5 + l_6)a_z \]

p5x = px - (L5 + L6) * ax;
p5y = py - (L5 + L6) * ay;
p5z = pz - (L5 + L6) * az;

從手臂機器人的結構來看，(From the structure of arm robot,)

\[\theta_1 = \arctan2(P_{5y},P_{5x}) \quad or \quad \theta_1 = \arctan2(P_{5y},P_{5x})+\pi \]

theta[0] = Mathf.Atan2(p5y, p5x);

形成手臂機器人的運動學 (Form the Kinematics of arm robot,)

\[\begin{align*} P_{5} &= \begin{bmatrix} 0 \\ 0 \\ l_{0}+l_{1}\end{bmatrix}\\\\ &+ \begin{bmatrix} C_{1} & -S_{1} & 0 \\ S_{1} & C_{1} & 0 \\ 0 & 0 & 1\end{bmatrix}\begin{bmatrix} C_{2} & 0 & S_{2} \\ 0 & 1 & 0 \\ -S_{2} & 0 & C_{2}\end{bmatrix}\begin{bmatrix} 0 \\ 0 \\ l_{2}\end{bmatrix}\\\\ &+ \begin{bmatrix} C_{1} & -S_{1} & 0 \\ S_{1} & C_{1} & 0 \\ 0 & 0 & 1\end{bmatrix}\begin{bmatrix} C_{23} & 0 & S_{23} \\ 0 & 1 & 0 \\ -S_{23} & 0 & C_{23}\end{bmatrix}\begin{bmatrix} 0 \\ 0 \\ l_{3}\end{bmatrix}\\\\ &+ \begin{bmatrix} C_{1} & -S_{1} & 0 \\ S_{1} & C_{1} & 0 \\ 0 & 0 & 1\end{bmatrix}\begin{bmatrix} C_{23} & 0 & S_{23} \\ 0 & 1 & 0 \\ -S_{23} & 0 & C_{23}\end{bmatrix}\begin{bmatrix} C_{4} & -S_{4} & 0 \\ S_{4} & C_{4} & 0 \\ 0 & 0 & 1\end{bmatrix}\begin{bmatrix} 0 \\ 0 \\ l_{4}\end{bmatrix}\\\\ &= \begin{bmatrix} 0 \\ 0 \\ l_{0}+l_{1}\end{bmatrix} + \begin{bmatrix} C_{1}S_{2} \\ S_{1}S_{2} \\ C_{2}\end{bmatrix}l_{2} + \begin{bmatrix} C_{1}S_{23} \\ S_{1}S_{23} \\ C_{23}\end{bmatrix}(l_{3} + l_{4}) \end{align*} \]

\[\begin{align*} P_{5x} &= C_{1}(S_{2}l_{2} + S_{23}(l_{3} + l_{4})) \tag{1}\\ P_{5y} &= S_{1}(S_{2}l_{2} + S_{23}(l_{3} + l_{4})) \tag{2}\\ P_{5z} - (l_{0} + l_{1}) &= C_{2}l_{2} + C_{23}(l_{3} + l_{4})\tag{3}\\ \end{align*} \]

平方和 (Sum of squares,)

\[\begin{align*} &(P_{5x})^2 + (P_{5y})^2 + (P_{5z} - (l_{0} + l_{1}))^2 \\ &= (S_{2}l_{2} + S_{23}(l_{3} + l_{4}))^2 + (C_{2}l_{2} + C_{23}(l_{3} + l_{4}))^2\\ &= l_{2}^2 + (l_{3} + l_{4})^2 + 2l_{2}(l_{3} + l_{4})(S_{2}S_{23} + C_{2}C_{23})\\ &= l_{2}^2 + (l_{3} + l_{4})^2 + 2l_{2}(l_{3} + l_{4})C_{3} \end{align*} \]

（以上使用餘弦定理）((Above used Cosine theorem))

\[\begin{align*} C_{3} = \frac{1}{2l_{2}(l_{3} + l_{4})}(P_{5x}^2 + P_{5y}^2 + (P_{5z} - (l_{0} + l_{1}))^2 - l_{2}^2 - (l_{3} + l_{4})^2) \end{align*} \]

因為 (for)

\[S^2 + C^2 = 1 \]

\[\begin{align*} \theta_{3} = atan2(\pm\sqrt{1 - C_{3}^2}, C_{3}) \end{align*} \]

C3 = (Mathf.Pow(p5x, 2) + Mathf.Pow(p5y, 2) + Mathf.Pow(p5z - L1, 2) - Mathf.Pow(L2, 2) - Mathf.Pow(L3+L4, 2)) / (2 * L2 * (L3+ L4));
theta[2] = Mathf.Atan2(Mathf.Pow(1 - Mathf.Pow(C3, 2), 0.5f), C3);

((①的平方+②的平方))和③的平方根，使用 (Squareroot of (squre of (1) + square of (2)) and (3),using)

\[S_{23} = S_{2}C_{3} + C_{2}S_{3}, C_{23} = C_{2}C_{3} - S_{2}S_{3} \]

\[\begin{align*} \sqrt{P_{5x}^2 + P_{5y}^2} &= (l_{2} + (l_{3} + l_{4})C_{3})S_{2} + ((l_{3} + l_{4})S_{3})C_{2}\\\\ P_{z5} - (l_{0} + l_{1}) &= - ((l_{3} + l_{4})S_{3})S_{2} + (l_{2} + (l_{3} + l_{4})C_{3})C_{2} \end{align*} \]

當我們定義時 (when we define)

\[\begin{align*} \sqrt{P_{5x}^2 + P_{5y}^2} \equiv A, \quad (l_{2} + (l_{3} + l_{4})C_{3}) \equiv M \\\\ P_{z5} - (l_{0} + l_{1}) \equiv B, \quad ((l_{3} + l_{4})S_{3}) \equiv N \end{align*} \]

\[\begin{align*} \begin{bmatrix} A \\ B \end{bmatrix} = \begin{bmatrix} M & N \\ -N & M\end{bmatrix}\begin{bmatrix} S_{2} \\ C_{2}\end{bmatrix} \end{align*} \]

得到 S₂ , C₂ (to get S₂ , C₂)

\[\begin{align*} \begin{bmatrix} S_{2} \\ C_{2} \end{bmatrix} = \frac{1}{M^2 + N^2}\begin{bmatrix} M & -N \\ N & M\end{bmatrix}\begin{bmatrix} A \\ B\end{bmatrix} \end{align*} \]

因此 θ₂ 是 (so θ₂ is)

\[\begin{align*} \theta_{2} = atan2(MA - NB, NA + MB) \end{align*} \]

float M = L2 + (L3+ L4) * C3;
float N = (L3+ L4) * Mathf.Sin((float)theta[2]);
float A = Mathf.Pow(p5x*p5x + p5y*p5y, 0.5f);
float B = p5z - L1;
theta[1] = Mathf.Atan2(M*A - N*B, N*A + M*B);

現在我們有 **θ1,θ2,θ3 (theta[0],theta[1],theta[2]) ** (Now we haveθ1,θ2,θ3 (theta[0],theta[1],theta[2]).)

末端執行器(a,b)的位姿與基座(a,b)的位姿的關係為 (Relation between pose of end effector (a,b)(a,b) and pose of base (^a,b)(a^,b) is)

\[\begin{align*} (a,b) = R_{1}R_{2}R_{3}R_{4}R_{5}R_{6}(\hat{a},\hat{b}) \end{align*} \]

應用反向旋轉(apply reverse rotations)

\[\begin{align*} R_{3}^{-1}R_{2}^{-1}R_{1}^{-1}(a,b) = R_{4}R_{5}R_{6}(\hat{a},\hat{b}) \end{align*} \]

左邊可以用已知的值來計算。(Left side can be calculated with know values.)

\[\begin{align*} &.\begin{bmatrix} C_{23} & 0 & -S_{23} \\ 0 & 1 & 0 \\ S_{23} & 0 & C_{23} \end{bmatrix}\begin{bmatrix} C_{1} & S_{1} & 0 \\ -S_{1} & C_{1} & 0 \\ 0 & 0 & 1 \end{bmatrix}\begin{bmatrix} a_{x} & b_{x} \\ a_{y} & b_{y} \\ a_{z} & b_{z}\end{bmatrix} \\\\ &= \begin{bmatrix} C_{23}(C_{1}a_{x} + S_{1}a_{y}) - S_{23}a_{z} & C_{23}(C_{1}b_{x} + S_{1}b_{y}) - S_{23}b_{z} \\ -S_{1}a_{x} + C_{1}a_{y} & -S_{1}b_{x} + C_{1}b_{y} \\ S_{23}(C_{1}a_{x} + S_{1}a_{y}) + C_{23}a_{z} & S_{23}(C_{1}b_{x} + S_{1}b_{y}) + C_{23}b_{z}\end{bmatrix} \\\\ &\equiv \begin{bmatrix}a_{x}^{*} & b_{x}^{*} \\ a_{y}^{*} & b_{y}^{*} \\ a_{z}^{*} & b_{z}^{*}\end{bmatrix} \end{align*} \]

右邊是 (Right side is)

\[\begin{align*} &.\begin{bmatrix} C_{4} & -S_{4} & 0 \\ S_{4} & c_{4} & 0 \\ 0 & 0 & 1 \end{bmatrix}\begin{bmatrix} C_{5} & 0 & S_{5} \\ 0 & 1 & 0 \\ -S_{5} & 0 & C_{5} \end{bmatrix}\begin{bmatrix} C_{6} & -S_{6} & 0 \\ S_{6} & C_{6} & 0 \\ 0 & 0 & 1 \end{bmatrix}\begin{bmatrix} 0 & 1 \\ 0 & 0 \\ 1 & 0\end{bmatrix} \\\\ &= \begin{bmatrix} C_{4}S_{5} & C_{4}C_{5}C_{6} - S_{4}S_{6} \\ S_{4}S_{5} & S_{4}C_{5}C_{6} + C_{4}S_{6} \\ C_{5} & -S_{5}C_{6}\end{bmatrix} \end{align*} \]

因此 θ₄,θ₅,θ₆ 都是 (So θ₄,θ₅,θ₆ are)

\[\begin{eqnarray} \left\{ \begin{array}{l} \theta_{4} = atan2(a_{y}^{*}, \ a_{x}^{*}) \quad or \quad atan2(a_{y}^{*}, \ a_{x}^{*}) + \pi\\ \theta_{5} = atan2(C_{4}a_{x}^{*} + S_{4}a_{y}^{*}, \ a_{z}^{*})\\ \theta_{6} = atan2(C_{4}b_{y}^{*} + S_{4}b_{x}^{*}, \ -b_{z}^{*}/S_{5})\\ \end{array} \right.\end{eqnarray} \]

C1 = Mathf.Cos((float)theta[0]);
C23 = Mathf.Cos((float)theta[1] + (float)theta[2]);
S1 = Mathf.Sin((float)theta[0]);
S23 = Mathf.Sin((float)theta[1] + (float)theta[2]);
 
asx = C23 * (C1 * ax + S1 * ay) - S23 * az;
asy = -S1 * ax + C1 * ay;
asz = S23 * (C1 * ax + S1 * ay) + C23 * az;
bsx = C23 * (C1 * bx + S1 * by) - S23 * bz;
bsy = -S1 * bx + C1 * by;
bsz = S23 * (C1 * bx + S1 * by) + C23 * bz;
 
theta[3] = Mathf.Atan2(asy, asx);
theta[4] = Mathf.Atan2(Mathf.Cos((float)theta[3]) * asx + Mathf.Sin((float)theta[3]) * asy, asz);
theta[5] = Mathf.Atan2(Mathf.Cos((float)theta[3]) * bsy - Mathf.Sin((float)theta[3]) * bsx,
            -bsz / Mathf.Sin((float)theta[4]));

END

UnityでアームロボットIK | unity | Meuse Robotics

網站原文1：

[のアムロボットシミュレショに]運動（反向運動學を導してみました]スライダーでアーム先端の位置と姿勢(6軸)を指定するとロボットがこれを満足するように動きます。

我知道の計は広瀬[ロボット]をにしました]あまり広範囲には動かせないようにしていますのでとりあえずうまくいっているようです。

翻譯：

[對Amrobot模擬]運動（我試圖指導反向運動）如果您用滑塊指定了臂尖的位置和姿勢（6個軸），則機器人將滿足此要求。

我完全選擇了Hirose [Robot]。]暫時看來它執行良好，因為我嘗試不將其移至很大範圍。

原始碼1

public class CalcIK : MonoBehaviour {
 
    public Slider S_Slider;    //PositionX min=4 max=12
    public Slider L_Slider;    //PositionY min=-4 max=4
    public Slider U_Slider;    //PositionZ min=4 max=12
    public Slider R_Slider;    //RotationX min=-90 max=90
    public Slider B_Slider;    //RotationY min=-90 max=90
    public Slider T_Slider;    //RotationZ min=-90 max=90
    public static double[] theta = new double[6];    //angle of the joints
 
    private float L1, L2, L3, L4, L5, L6;    //arm length in order from base
    private float C3;
 
    void Start () {
        theta[0] = theta[1] = theta[2] = theta[3] = theta[4] = theta[5] = 0.0;
        L1 = 4.0f;
        L2 = 6.0f;
        L3 = 3.0f;
        L4 = 4.0f;
        L5 = 2.0f;
        L6 = 1.0f;
        C3 = 0.0f;
    }
     
    void Update () {
        float px, py, pz;
        float rx, ry, rz;
        float ax, ay, az, bx, by, bz;
        float asx, asy, asz, bsx, bsy, bsz;
        float p5x, p5y, p5z;
        float C1, C23, S1, S23;

        px = S_Slider.value;
        py = L_Slider.value;
        pz = U_Slider.value;
        rx = R_Slider.value;
        ry = B_Slider.value;
        rz = T_Slider.value;
 
        ax = Mathf.Cos(rz * 3.14f / 180.0f) * Mathf.Cos(ry * 3.14f / 180.0f);
        ay = Mathf.Sin(rz * 3.14f / 180.0f) * Mathf.Cos(ry * 3.14f / 180.0f);
        az = -Mathf.Sin(ry * 3.14f / 180.0f);

        p5x = px - (L5 + L6) * ax;
        p5y = py - (L5 + L6) * ay;
        p5z = pz - (L5 + L6) * az;
 
        theta[0] = Mathf.Atan2(p5y, p5x);
 
        C3 = (Mathf.Pow(p5x, 2) + Mathf.Pow(p5y, 2) + Mathf.Pow(p5z - L1, 2) - Mathf.Pow(L2, 2) - Mathf.Pow(L3+L4, 2))
            / (2 * L2 * (L3+ L4));
        theta[2] = Mathf.Atan2(Mathf.Pow(1 - Mathf.Pow(C3, 2), 0.5f), C3);
 
        float M = L2 + (L3+ L4) * C3;
        float N = (L3+ L4) * Mathf.Sin((float)theta[2]);
        float A = Mathf.Pow(p5x*p5x + p5y*p5y, 0.5f);
        float B = p5z - L1;
        theta[1] = Mathf.Atan2(M*A - N*B, N*A + M*B);
 
        C1 = Mathf.Cos((float)theta[0]);
        C23 = Mathf.Cos((float)theta[1] + (float)theta[2]);
        S1 = Mathf.Sin((float)theta[0]);
        S23 = Mathf.Sin((float)theta[1] + (float)theta[2]);
 
        bx = Mathf.Cos(rx * 3.14f / 180.0f) * Mathf.Sin(ry * 3.14f / 180.0f) * Mathf.Cos(rz * 3.14f / 180.0f)
            - Mathf.Sin(rx * 3.14f / 180.0f) * Mathf.Sin(rz * 3.14f / 180.0f);
        by = Mathf.Cos(rx * 3.14f / 180.0f) * Mathf.Sin(ry * 3.14f / 180.0f) * Mathf.Sin(rz * 3.14f / 180.0f)
            - Mathf.Sin(rx * 3.14f / 180.0f) * Mathf.Cos(rz * 3.14f / 180.0f);
        bz = Mathf.Cos(rx * 3.14f / 180.0f) * Mathf.Cos(ry * 3.14f / 180.0f);
 
        asx = C23 * (C1 * ax + S1 * ay) - S23 * az;
        asy = -S1 * ax + C1 * ay;
        asz = S23 * (C1 * ax + S1 * ay) + C23 * az;
        bsx = C23 * (C1 * bx + S1 * by) - S23 * bz;
        bsy = -S1 * bx + C1 * by;
        bsz = S23 * (C1 * bx + S1 * by) + C23 * bz;
 
        theta[3] = Mathf.Atan2(asy, asx);
        theta[4] = Mathf.Atan2(Mathf.Cos((float)theta[3]) * asx + Mathf.Sin((float)theta[3]) * asy, asz);
        theta[5] = Mathf.Atan2(Mathf.Cos((float)theta[3]) * bsy - Mathf.Sin((float)theta[3]) * bsx,
            -bsz / Mathf.Sin((float)theta[4]));
    }
}

Unityでアームロボットをシミュレーション | unity | Meuse Robotics

網站原文2：

安川電機のアームロボットを遠隔操作する案件があり、基本的にはWebカメラで見るのですが、ロボットの現在狀況を3Dで表示したいということでUnityを使ってみました。
Unityに関しては、Courseraのゲーム開発のコースを途中まで視聴した程度のレベルです。ですのでシミュレーションといってもアニメーションを表示するだけのものです。

ここではわかりやすくするためにロボットの関節角度をUnityのUIスライダーで與えています。実際にはロボットの関節角度データはロボットのコントローラからもらいます.

臺座から手先に向けて順番に入れ子構造にしていくことで、ある関節を回した時にそこから先のアームを一體で動かすことができます。各アームは根元から、S、L、U、R、B、Tと名前がついています。アームにはそれぞれ円柱部材（Axis）があって、それより手先側のアセンブリの回転軸となっています。

![arm_robot_unity_2](Unity Arm Robot Inverse Kinematics -analytical approach.assets/arm_robot_unity_2-273x300.png)

Cadのデータを使ってかっこよくしたものがこちらです。

![arm_robot_unity_3](Unity Arm Robot Inverse Kinematics -analytical approach.assets/arm_robot_unity_3-300x191.png)

描畫フレームごとにスクリプト（RotationScript.cs）で各軸の回転処理を行います。

翻譯

有一個可以遠端控制Yasukawa Electric手臂機器人的專案，我基本上可以通過網路攝像頭看到它，但是我想以3D方式顯示機器人的當前狀態，因此我嘗試使用Unity。
至於Unity，則是大約觀看Coursera的遊戲開發課程的水平。因此，模擬只是顯示動畫。

在這裡，為清楚起見，機器人的關節角度由Unity UI滑塊給出。實際上，機器人的關節角度資料是從機器人的控制器獲得的。

通過從基座到手按順序建立巢狀結構，當轉動某個關節時，可以整體移動超出該範圍的手臂。從根開始，每個手臂都被命名為S，L，U，R，B，T。每個臂都有一個圓柱形的構件（軸），該構件是手側元件的旋轉軸。

這是一個使用Cad資料的不錯的版本。

使用每個圖紙框架的指令碼（RotationScript.cs）旋轉每個軸。

原始碼：

public class RotationScript : MonoBehaviour
{
    public Transform target;
    public Vector3 direction;
    public int axisNum;
    private Vector3 targetPos;
    private float[] data = new float[6];
    private float[] angle = new float[6];
     
    public Slider S_Slider;
    public Slider L_Slider;
    public Slider U_Slider;
    public Slider R_Slider;
    public Slider B_Slider;
    public Slider T_Slider;
     
    void Start()
    {
        for (int i = 0; i < 6; i++)
        {
            angle[i] = 0;
        }
    }
 
    void Update()
    {
        data[0] = S_Slider.value;
        data[1] = L_Slider.value;
        data[2] = U_Slider.value;
        data[3] = R_Slider.value;
        data[4] = B_Slider.value;
        data[5] = T_Slider.value;
        if (angle[axisNum] != data[axisNum])
        {
            targetPos = target.position;
            transform.RotateAround(targetPos, target.up, data[axisNum]-angle[axisNum]);
            angle[axisNum] = data[axisNum];
        }
    }
}

國外網站參考：

Meuse Robotics

愛好機器人和電子玩具的規劃和開發 • 嵌入式電路設計、軟體開發
• 機電一體化裝置開發和設計
• 製造商大規模生產設計支援
• 工業臂機器人控制軟體開發

機器人、逆運動學：

UnitySir - [Unity模擬] 逆運動學

視訊：

Final IK

單臂機器人逆運動學解析方法

逆運動學解析正文：

網站原文1：

翻譯：

原始碼1

網站原文2：

翻譯

原始碼：

國外網站參考：

個人資訊：

姓名：鄒建

19年加入 四川省裝備製造機器人應用技術工程實驗室，Dream Studio 軟體工作室，負責軟體開發相關工作

擅長：Unity遊戲開發，Unity機器人模擬，C#程式語言，工業軟體開發

QQ：451991189

B站ID：UnitySir

個人部落格：

B站ID：UnitySir

如果內容對你有所幫助：

相關推薦

19年加入四川省裝備製造機器人應用技術工程實驗室，Dream Studio 軟體工作室，負責軟體開發相關工作