Công thức môn Kỹ thuật robot

Kỹ thuật robot CÔNG THỨC CƠ BẢN 1 Vecto mô tả vị trí

CƠNG THỨC CƠ BẢN Vecto mơ tả vị trí: 𝐴 𝑝𝑥 𝑃 = [𝑝𝑦 ] 𝑝𝑧 Vecto định hướng: 𝐴 𝐵𝑅 𝐴̂ 𝑌𝐵 𝐴 𝑟11 ̂ 𝑟 𝑍𝐵 ] = [ 21 𝑟31 𝐴̂ 𝑌𝐵 𝐴 ̂𝐵 𝑋 ̂𝐴 𝑋 ̂𝐵 𝑌̂𝐴 𝑍̂𝐵 ] = [ 𝑋 ̂𝐵 𝑍 ̂𝐴 𝑋 ̂ ̂ 𝑌 𝐵 𝑋𝐴 ̂ ̂ 𝑌 𝐵 𝑌𝐴 ̂ ̂ 𝑌 𝐵 𝑍𝐴 ̂𝐵 𝑋 ̂𝐴 𝑍 ̂𝐵 𝑌̂𝐴 ] 𝑍 ̂𝐵 𝑍 ̂𝐴 𝑍 𝐵̂ 𝑌𝐴 𝐵 ̂𝐴 𝑋 ̂𝐵 𝑋 ̂𝐴 𝑌 ̂ 𝑍̂𝐴 ] = [ 𝑋 𝐵 ̂ ̂ 𝑋𝐴 𝑍𝐵 ̂𝐵 𝑌̂𝐴 𝑋 ̂ 𝑌̂𝐴 𝑌 𝐵 ̂ ̂ 𝑌𝐴 𝑍𝐵 ̂𝐴 𝑋 ̂𝐵 𝑍 ̂𝐴 𝑌 ̂ 𝑍 𝐵] ̂ ̂ 𝑍𝐴 𝑍𝐵 𝐴 = [ 𝑋̂𝐵 𝑟12 𝑟22 𝑟32 𝑟13 𝑟23 ] 𝑟33 B chiếu lên A: 𝐴 𝐵𝑅 𝐴 = [ 𝑋̂𝐵 A chiếu lên B: 𝐵 𝐴𝑅 𝐵 = [ 𝑋̂𝐴 Tích vơ hướng vecto: ̂𝐴 𝑋 ̂𝐵 = |𝑋 ̂𝐴 | |𝑋 ̂𝐵 | cos⁡(𝑋 ̂𝐴 , 𝑋 ̂𝐵 ) 𝑋 Lưu ý: Trong ma trận xoay, hàng / cột vectơ đơn vị trực giao |𝑋̂| = 𝑋̂ 𝑌̂ = {|𝑌̂| = ⁡𝑣à⁡ {𝑋̂ 𝑍̂ = 𝑌̂ 𝑍̂ = |𝑍̂| = Homogeneous transform 𝐴 𝑃 = 𝐵𝐴𝑇 𝐵𝑃 𝐴 𝐴 [ 𝑃 ] = [ 𝐵𝑅 0 𝐴 𝐴 𝑃𝐵𝑂𝑅𝐺 ] [ 𝐵𝑃] 1 𝑃2 = 𝐷𝑄 (𝑞) 𝑅𝐾 (𝜃) 𝐴𝑃1 Toán tử tịnh tiến 𝐴 𝑃2 = 𝐴𝑃1 + 𝐴𝑄 𝐴 𝑄 :⁡ma⁡trận⁡trịnh⁡tiến⁡ Hoặc viết dạng: DUC LINH HONG 𝐴 𝑃2 = 𝐷𝑄 (𝑞) 𝐴𝑃1 𝐷𝑄 (𝑞) = [ 0 0 0 𝑞𝑥 𝑞𝑦 ] :⁡như⁡một⁡ma⁡trận⁡T⁡đặc⁡biệt 𝑞𝑧 Toán tử xoay: 𝐴 𝑃2 = 𝑅𝐾 (𝜃) 𝐴𝑃1 0 𝑐𝑜𝑠(𝜃) 𝑅𝑋 (𝜃) = [ 𝑠𝑖𝑛(𝜃) 0 𝑐𝑜𝑠(𝜃) 𝑅𝑌 (𝜃) = [ −𝑠𝑖𝑛(𝜃) 0 −𝑠𝑖𝑛(𝜃) cos(𝜃) 0 ] 𝑠𝑖𝑛(𝜃) 0] cos(𝜃) 0 𝑐𝑜𝑠(𝜃) −𝑠𝑖𝑛(𝜃) 0 𝑅𝑍 (𝜃) = [ 𝑠𝑖𝑛(𝜃) 𝑐𝑜𝑠(𝜃) 0] 0 0 0 Chú ý: 𝐷𝑋 (𝑞1 )𝐷𝑌 (𝑞2 ) = 𝐷𝑌 (𝑞2 )𝐷𝑋 (𝑞1 ) 𝑅𝑋 (𝜃1 )𝑅𝑌 (𝜃2 ) ≠ 𝑅𝑌 (𝜃2 )𝑅𝑋 (𝜃1 ) GÓC ROLL – PITCH – YAW (X – Y – Z FIXED ANGLES) Cho hệ trục gốc {A}, {B} trùng với {A} Xoay hệ trục {B} quanh 𝑋𝐴 góc 𝛾⁡(roll) DUC LINH HONG Xoay hệ trục {B} quanh 𝑌𝐴 góc 𝛽⁡(pitch) Xoay hệ trục {B} quanh 𝑍𝐴 góc 𝛼⁡(yaw) 𝐴 𝐵𝑅𝑋𝑌𝑍 (𝛾, 𝛽, 𝛼) 𝑐𝛼𝑐𝛽 = 𝑅𝑍 (𝛼)𝑅𝑌 (𝛽)𝑅𝑋 (𝛾) = [𝑠𝛼𝑐𝛽 −𝑠𝛽 𝑐𝛼𝑠𝛽𝑠𝛾 − 𝑠𝛼𝑐𝛾 𝑐𝛼𝑐𝛾 + 𝑠𝛼𝑠𝛽𝑠𝛾 𝑐𝛽𝑠𝛾 𝑠𝛼𝑠𝛾 + 𝑐𝛼𝑠𝛽𝑐𝛾 𝑠𝛼𝑠𝛽𝑐𝛾 − 𝑐𝛼𝑠𝛾 ] 𝑐𝛽𝑐𝛾 GÓC EULER ( Z-Y-Z EULER ANGLES) Cho hệ trục gốc {A}, {B} trùng với {A} Xoay hệ trục {B} quanh 𝑍𝐵 góc 𝛼⁡ Xoay hệ trục {B} quanh 𝑌𝐵 góc 𝛽⁡ Xoay hệ trục {B} quanh 𝑋𝐵 góc 𝛾⁡ 𝐴 𝐵𝑅𝑍′𝑌′𝑋′ (𝛼, 𝛽, 𝛾) 𝑐𝛼𝑐𝛽 = 𝑅𝑍 ′(𝛼)𝑅𝑌 ′(𝛽)𝑅𝑋′ (𝛾) [𝑠𝛼𝑐𝛽 −𝑠𝛽 𝑐𝛼𝑠𝛽𝑠𝛾 − 𝑠𝛼𝑐𝛾 𝑐𝛼𝑐𝛾 + 𝑠𝛼𝑠𝛽𝑠𝛾 𝑐𝛽𝑠𝛾 𝑠𝛼𝑠𝛾 + 𝑐𝛼𝑠𝛽𝑐𝛾 𝑠𝛼𝑠𝛽𝑐𝛾 − 𝑐𝛼𝑠𝛾 ] 𝑐𝛽𝑐𝛾 Tính góc: Nếu cos(𝛽) ≠ 𝛾 = 𝑎𝑟𝑐𝑡𝑎𝑛(𝑟32 , 𝑟33 ) 2 → 𝛽 = arctan⁡(−𝑟31 , √𝑟11 + 𝑟21 ) { 𝛼 = arctan⁡(𝑟21 , 𝑟11 ) Nếu cos(𝛽) = 𝛾 = 𝑎𝑟𝑐𝑡𝑎𝑛(𝑟12 , 𝑟22 ) 𝛾 = −𝑎𝑟𝑐𝑡𝑎𝑛(𝑟12 , 𝑟22 ) →{ ℎ𝑜ặ𝑐 { ⁡ 𝛽 = 90 𝛽 = −900 0 𝛼=0 𝛼=0 DUC LINH HONG HỆ TRỤC TƯƠNG ĐƯƠNG Cho hệ trục gốc {A}, {B} trùng với {A} Sau xoay {B} vecto 𝐴𝐾 góc 𝜃 theo quy tắc bàn tay phải Ma trận xoay: 𝑘𝑥 𝑘𝑥 𝑣𝜃 + 𝑐𝜃 𝑅𝐾 (𝜃) = [𝑘𝑥 𝑘𝑦 𝑣𝜃 + 𝑘𝑧 𝑠𝜃 𝑘𝑥 𝑘𝑧 𝑣𝜃 − 𝑘𝑦 𝑠𝜃 𝑘𝑥 𝑘𝑦 𝑣𝜃 − 𝑘𝑧 𝑠𝜃 𝑘𝑦 𝑘𝑦 𝑣𝜃 + 𝑐𝜃 𝑘𝑦 𝑘𝑧 𝑣𝜃 + 𝑘𝑥 𝑠𝜃 𝑘𝑥 𝑘𝑧 𝑣𝜃 + 𝑘𝑦 𝑠𝜃 𝑘𝑦 𝑘𝑧 𝑣𝜃 − 𝑘𝑥 𝑠𝜃] 𝑘𝑧 𝑘𝑧 𝑣𝜃 + 𝑐𝜃 Với 𝑣𝜃 = − 𝑐𝑜𝑠𝜃 Nếu 𝐴 𝐵𝑅 𝑟11 𝑟 = [ 21 𝑟31 𝑟12 𝑟22 𝑟32 𝑟13 𝑟23 ] 𝑟33 𝑟11 + 𝑟22 + 𝑟33 − ) 𝑟32 − 𝑟23 ̂ 𝐾= [𝑟13 − 𝑟31 ] 2𝑠𝑖𝑛𝜃 𝑟 − 𝑟 𝜃 = 𝑎𝑟𝑐𝑐𝑜𝑠 ( → { 21 12 ĐẶT TỌA ĐỘ - Trục 𝑍𝑖 trùng với trục khớp i+1 - Trục 𝑋𝑖 theo hướng trục khớp i tới khớp i+1, đường vng góc chung trục Z Nếu trục Z vuông góc X qua giao điểm vng góc với mặt phẳng trục Z - Trục 𝑌𝑖 theo hướng quy tắc tam diện thuận - Hệ trục 𝑋0 𝑌0 𝑍0 hệ trục khâu liên kết với đất, chọn tự do, chọn cho thuận tiện tính tốn Ví dụ: DUC LINH HONG BẢNG DH 𝒂𝒊 𝑎1 𝑎2 ⋮ 𝑎n−1 Link ⋮ n-1 𝜶𝒊 𝛼1 𝛼2 ⋮ 𝛼n−1 𝒅𝒊 𝑑1 𝑑2 ⋮ 𝑑n−1 𝜽𝒊 𝜃1 𝜃2 ⋮ 𝜃n−1 - 𝒂𝒊 : khoảng cách từ trục 𝑍𝑖−1 ⁡tới 𝑍𝑖 dọc theo trục 𝑋𝑖 -⁡𝜶𝒊 : góc từ trục 𝑍𝑖−1 tới 𝑍𝑖 nhìn từ trục 𝑋𝑖 - ⁡𝒅𝒊 : khoảng cách từ trục 𝑋𝑖−1 ⁡tới 𝑋𝑖 dọc theo trục 𝑍𝑖−1 - ⁡𝜽𝒊 : góc từ trục 𝑋𝑖−1 tới 𝑋𝑖 nhìn từ trục 𝑍𝑖−1 MA TRẬN CHUYỂN ĐỔI THUẦN NHẤT (the neighboring homogeneous transformation matrices) 𝑐𝑜𝑠𝜃𝑖 𝑠𝑖𝑛𝜃𝑖 𝑖−1 𝑖𝑇 = [ 0 −𝑠𝑖𝑛𝜃𝑖 𝑐𝑜𝑠𝛼𝑖 𝑐𝑜𝑠𝜃𝑖 𝑐𝑜𝑠𝛼𝑖 𝑠𝑖𝑛𝛼𝑖 𝑠𝑖𝑛𝜃𝑖 𝑠𝑖𝑛𝛼𝑖 −𝑐𝑜𝑠𝜃𝑖 𝑠𝑖𝑛𝛼𝑖 𝑐𝑜𝑠𝛼𝑖 𝑎𝑖 𝑐𝑜𝑠𝜃𝑖 𝑎𝑖 𝑠𝑖𝑛𝜃𝑖 ] 𝑑𝑖 Ma trận vector cuối: 𝑟11 𝑟21 0 𝑁−1 𝑁𝑇 = 1𝑇 2𝑇 … 𝑁𝑇=[𝑟 31 𝑟12 𝑟22 𝑟32 𝑟13 𝑟23 𝑟33 𝑝𝑥 𝑝𝑦 ] 𝑝𝑧 DUC LINH HONG JACOBIAN Ma trận Jacobian: 𝑉 = 0𝐽(𝜃) 𝜃̇ ↔ 𝜃̇ = 0𝐽(𝜃) 0𝑉 𝜃1̇ ℎ𝑜ặ𝑐⁡𝑑1̇ 𝑋̇ → [𝑌̇ ] = 0𝐽 [𝜃2̇ ℎ𝑜ặ𝑐⁡𝑑2̇ ] ̇ 𝑍̇ ̇ 𝜃3 ℎ𝑜ặ𝑐⁡𝑑 Từ ma trận 𝑁0𝑇 ta có vị trí x, y, z 𝑝𝑥 𝑝𝑦 , 𝑝𝑧 Lấy đạo hàm xong rút 𝜃1̇ , 𝜃2̇ , 𝜃3̇ , 𝑑1̇ , 𝑑2̇ , 𝑑3̇ ma trận 0𝐽 3x3 Điểm kì dị: Là vị trí mà 𝐷𝑒𝑡(𝐽) = Tất robot có điểm kì dị nằm vị trí biên ( góc 180), số có điểm nằm vùng hoạt động Jacobian cho lực: 𝜏 = 𝐽𝑇 𝐹 Trong đó: 𝐹: lực tác dụng lên khớp 𝜏: moment 𝐽𝑇 : chuyển vị ma trận Jacobian Cách – không dạy – biết cho vui – khơng vui thui – làm dễ Tính ma trận 1→𝑛 𝑇 1𝑇 =[ ] 2𝑇 =[ ] … 𝑅11 𝑅 21 𝑛𝑇 = [ 𝑅31 𝑅12 𝑅22 𝑅32 𝑅13 𝑅23 𝑅33 𝑝𝑥 𝑝𝑦 ] 𝑝𝑧 + Hàng 1:    Robot có n khớp Nếu khớp xoay hàng đầu 𝑧𝑖 (𝑝𝑛 − 𝑝𝑖 ) Nếu khớp tịnh tiến hàng đầu 𝑧𝑖 DUC LINH HONG + Hàng giá trị 𝑧𝑖 + Ví dụ robot có khớp: khớp xoay, khớp tịnh tiến khâu cuối 𝐽 =⁡[ 𝑧0 (𝑝3 − 𝑝0 ) 𝑧1 (𝑝3 − 𝑝1 ) 𝑧2 ] 𝑧0 𝑧1 𝑧2 𝑧0 = [0] ;⁡𝑧1 ; 𝑧2 : giá trị đầu hàng ma trận 1→2𝑇 (𝑅13 , 𝑅23 , 𝑅33 ) 𝑝0 = [0] ; 𝑝1 ;⁡𝑝2 ; 𝑝3 ⁡: giá trị vị trí ma trận 1→30𝑇 (𝑝𝑥 , 𝑝𝑦 , 𝑝𝑧 ) CÁC CÔNG THỨC BỔ TRỢ TÍNH TỐN Tính bậc tự do: 𝑗 𝐹 = 𝜆(𝑛 − 𝑗 − 1) − ∑ 𝑓𝑖 𝑖=1⁡ Trong đó: 𝑓𝑖 : số bậc tự khớp i j: số khớp n: số khâu (đất/giá tính chung khâu) 𝜆: số bậc tự không gian mà cấu hoạt động = Tính định thức ma trận: Cho 𝐴 = (𝑎𝑖𝑗 ) ∈ 𝑀𝑛 (𝐾) Định thức ma trận A kí hiệu Det(A) hay |A| tính cơng thức: det(𝐴) = |𝐴| = 𝑎11 𝐴11 + 𝑎12 𝐴12 + ⋯ + 𝑎1𝑛 𝐴1𝑛 Trong đó: 𝐴𝑖𝑘 = (−1)𝑖+𝑘 det(𝑀𝑖𝑘 ) 𝑀𝑖𝑘 ma trận vuông cấp n-1 nhận từ ma trận A cách bỏ dòng thứ i cột thứ k Đại lượng 𝐴𝑖𝑘 gọi phần bù đại số 𝑎𝑖𝑘 - Ma trận 1x1: 𝐴 = (𝑎11 ) → 𝑑𝑒𝑡𝐴 = 𝑎11 - Ma trận 2x2: 𝑎 𝑑𝑒𝑡𝐴 = [ 𝑐 𝑏 ] = 𝑎𝑑 − 𝑏𝑐 𝑑 DUC LINH HONG - Ma trận 3x3: 𝑎 𝑑𝑒𝑡𝐴 = [𝑑 𝑔 𝑏 𝑒 ℎ 𝑐 𝑓 ] = 𝑎(𝑒𝑖 − ℎ𝑓) − 𝑏(𝑑𝑖 − 𝑓𝑔) + 𝑐(𝑑ℎ − 𝑒𝑔) 𝑖 Công thức lượng giác: + 𝑠𝑖𝑛⁡(a⁡ ± ⁡b) ⁡ = ⁡𝑠𝑖𝑛 a 𝑐𝑜𝑠 b ± ⁡𝑐𝑜𝑠 a 𝑠𝑖𝑛 b + 𝑐𝑜𝑠 (a⁡ + ⁡b) = ⁡𝑐𝑜𝑠 a 𝑐𝑜𝑠 b⁡ − 𝑠𝑖𝑛 a 𝑠𝑖𝑛 b + 𝑐𝑜𝑠⁡(a⁡ − ⁡b) ⁡ = ⁡𝑐𝑜𝑠 a 𝑐𝑜𝑠 b + ⁡𝑠𝑖𝑛 a 𝑠𝑖𝑛 b + 𝑐𝑜𝑠𝑎 𝑐𝑜𝑠𝑏 = [cos(𝑎 + 𝑏) + cos(𝑎 − 𝑏)] + 𝑠𝑖𝑛𝑎 𝑐𝑜𝑠𝑏 = − [sin(𝑎 + 𝑏) + sin(𝑎 − 𝑏)] + 𝑠𝑖𝑛𝑎 𝑠𝑖𝑛𝑏 = − [cos(𝑎 + 𝑏) − cos(𝑎 − 𝑏)] DUC LINH HONG ... số bậc tự không gian mà cấu hoạt động = Tính định thức ma trận: Cho