ฉันจะแปลงพารามิเตอร์ลิงก์และมุม (เป็น kinematics) เป็นเมทริกซ์การแปลงในตรรกะการเขียนโปรแกรมได้อย่างไร

ฉันกำลังทำวิจัยหุ่นยนต์ในระดับปริญญาตรีและฉันเข้าใจคณิตศาสตร์แนวความคิดเป็นส่วนใหญ่ อย่างไรก็ตามเมื่อพูดถึงการนำรหัสมาใช้จริงในการคำนวณจลศาสตร์การเคลื่อนที่ไปข้างหน้าสำหรับหุ่นยนต์ของฉันฉันก็ติดอยู่ ฉันแค่ไม่เข้าใจวิธีที่หนังสือหรือเว็บไซต์ที่ฉันพบอธิบาย

ฉันต้องการคำนวณมุม XYZ ที่กำหนดพารามิเตอร์ลิงก์ (พารามิเตอร์ Denavit-Hartenberg) เช่นต่อไปนี้ :

$$\begin{array}{ccc} \bf{i} & \bf{\alpha_i-1} & \bf{a_i-1} & \bf{d_i} & \bf{\theta_i}\\ \\ 1 & 0 & 0 & 0 & \theta_1\\ 2 & -90^{\circ} & 0 & 0 & \theta_2\\ 3 & 0 & a_2 & d_3 & \theta_3\\ 4 & -90^{\circ} & a_3 & d_4 & \theta_4\\ 5 & 90^{\circ} & 0 & 0 & \theta_5\\ 6 & -90^{\circ} & 0 & 0 & \theta_6\\ \end{array}$$

ฉันไม่เข้าใจวิธีเปลี่ยนค่าของตารางนี้เป็นเมทริกซ์การแปลงที่เหมาะสมเพื่อรับ , ตำแหน่งคาร์ทีเซียนและการหมุนของลิงก์สุดท้าย จากตรงนั้นฉันหวังว่าฉันจะสามารถหามุม XYZ จากการอ่านหนังสือของฉันได้ แต่ความช่วยเหลือจะได้รับการชื่นชม$^0T_N$

ส่วนDH Matrixของหน้า DH ในวิกิพีเดียมีรายละเอียด

โดยทั่วไปคุณต้องการใช้ข้อมูลในตารางของคุณเพื่อสร้างชุดเมทริกซ์การแปลงแบบเอกพันธ์ เราทำเช่นนี้เพราะการแปลงแบบเอกพันธ์สามารถถูกคูณเพื่อหาความสัมพันธ์ระหว่างเฟรมที่แยกจากเฟรมอื่น ๆ อย่างน้อยหนึ่งเฟรม ยกตัวอย่างเช่นแสดงให้เห็นถึงการเปลี่ยนแปลงจาก 1 กรอบกรอบ 0 ในขณะที่แสดงให้เห็นถึงการเปลี่ยนแปลงจากกรอบ 2 กรอบ 1 โดยคูณพวกเขาที่เราได้รับการเปลี่ยนแปลงจากกรอบ 2 กรอบ 0 คือ^$^0T_1$$^1T_2$$^0T_2 = ^0T_1^1T_2$

วิธีง่ายๆในการสร้างการแปลงแต่ละครั้งคือการแปลงแบบเอกพันธ์หรือเมทริกซ์การหมุนแบบเอกพันธ์สำหรับแต่ละคอลัมน์ในตารางและคูณพวกมันเข้าด้วยกัน ตัวอย่างเช่นการแปลงจาก 1 เป็น 0 (เช่น ) คือ$^{i-1}T_i, i = 1$

$^0T_1 = Trans(d_1)*Rot(\theta_1)*Trans(a_2)*Rot(\alpha_2)$

ที่ไหน

$Trans(d_1) = \begin{bmatrix}1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & \bf{d_1 = 0} \\ 0 & 0 & 0 & 1 \end{bmatrix},$

$Rot(\theta_1) = \begin{bmatrix} \text{cos}(\bf{\theta_1}) & - \text{sin}(\bf{\theta_1}) & 0 & 0 \\ \text{sin}(\bf{\theta_1}) & \text{cos}(\bf{\theta_1}) & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix},$

$Trans(a_2) = \begin{bmatrix} 1 & 0 & 0 & \bf{a_2 = 0} \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix},$

$Rot(\alpha_2) = \begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & \text{cos}(\bf{\alpha_2 = 0}) & -\text{sin}(\bf{\alpha_2 = 0}) & 0 \\ 0 & \text{sin}(\bf{\alpha_2 = 0}) & \text{cos}(\bf{\alpha_2 = 0}) & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix}${bmatrix}

ในกรณีนี้

$^0T1 = Rot(\theta_1)$theta_1)

เมื่อคุณมีการแปลงทั้งหมดของคุณคุณจะคูณพวกเขาไปอีกเช่น

$^0T_N = ^0T_1*^1T_2...^{N-1}T_N$^

ในที่สุดคุณสามารถอ่านการกำจัดเวกเตอร์จากการแปลงแบบเอกพันธ์ (เช่น ) คุณสามารถอ่านเมทริกซ์การหมุนได้จากเพื่อหามุม XYZ$^0T_N$$d = [^0T_{N,14}, ^0T_{N,24}, ^0T_{N,34}]^T$$^0T_N$