T 6= (2.2) ổng quát, ma trận 6 có thể biểu diễn gọn hơn như sau :
2.2.2. Lời giải của phép biến đổi Euler
Phép biến đổi Euler để mô tả hướng của khâu chấp hành cuối : Euler (Φ,θ,ψ) = Rot(z, Φ) Rot(y, θ) Rot(z, ψ)
Tập nghiệm muốn tìm là các góc Φ, θ, ψ khi đã biết ma trận biến đổi đồng nhất T6 (còn gọi là ma trận vectơ cuối). Nếu ta có các giá trị số của các phần tử trong ma trận T6 thì có thể xác định được các góc Euler Φ, θ, ψ thích hợp. Như vậy ta có :
Euler (Φ,θ,ψ) = T6 (2.7)
Vế trái của phương trình đã được biểu diễn bằng công thức , nên ta có :
=
(2.8)
Lần lượt cho cân bằng các phần tử tương ứng của hai ma trận trong phương trình (3.8) ta có các phương trình sau : nx = cosΦCosθcosψ - sinΦsinψ (2.9) ny = sinΦCosθcosψ + cosΦsinψ (2.10) nz = -sinθ cosψ (2.11) Ox = -cosΦCosθsinψ - sinΦcosψ (2.12) Oy = -sinΦCosθsinψ + cosΦcosψ (2.13) Oz = sinθ sinψ (2.14) ax = cosΦsinθ (2.15) ay = sinΦsinθ (2.16) az = cosθ (2.17)
Ta thử giải hệ phương trình này để tìm Φ, θ, ψ như sau :
Từ (2.15) ta có Φ = cos-1(ax / sinθ) (2.19)
Từ (2.11) và (2.18) ta có ψ = cos-1(-nz / sinθ) (2.20)
Trong đó ta đã dùng ký hiệu cos-1 thay cho hàm arccos.
Nhưng các kết quả đã giải ở trên chưa dùng được vì các lý do dưới đây : + Hàm arccos không chỉ biểu hiện cho một góc chưa xác định mà về độ chính xác nó lại phụ thuộc vào chính góc đó, nghĩa là :
cosθ = cos(-θ) : θ chưa được xác định duy nhất. | 0,180 = 0 : θ xác định không chính xác.
+ Trong lời giải đối với Φ và ψ một lần nữa chúng ta lại dùng hàm arccos và chia cho sinθ, điều này dẫn tới sự mất chính xác khi θ có giá trị lân cận 0.
+ Các phương trình (2.19) và (2.20) không xác định khi θ = 0 hoặc θ = ±1800. Do vậy chúng ta cần phải cẩn thận hơn khi chọn lời giải. Để xác định các góc khi giải bài toán ngược của robot ta phải dùng hàm arctg2 (y,x) (hàm arctang hai biến). Hàm arctg2 nhằm mục đích xác định được góc thực - duy nhất khi xét dấu của hai biến y và x. Hàm số trả về giá trị góc trong khoảng -π ≤ θ < π.
Ví dụ :
arctg2(-1/-1)= -1350, trong khi arctg2(1/1) = 450. Hàm này xác định ngay cả khi x hoặc y bằng 0 và cho kết quả đúng.
(Trong một số ngôn ngữ lập trình như Matlab, turbo C++, Maple hàm arctg2(y,x) đã có sẵn trong thư viện)
Để có thể nhận được những kết quả chính xác của bài toán Euler, ta thực hiện thủ thuật toán học sau : Nhân T6 với ma trận quay nghịch đảo Rot(z, F)-1,ta có:
Rot(z, Φ)-1T6 = Rot(y, θ) Rot(z, ψ) (2.21)
Vế trái của phương trình (2.21) là một hàm số của ma trận T và góc quay Φ. Ta thực hiện phép nhân ma trận ở vế phải của phương trình (2.21), tìm ra các phần tử của ma trận có giá trị bằng 0 hoặc bằng hằng số, cho các phần tử nầy cân bằng với những phần tử tương ứng của ma trận ở vế trái, cụ thể từ (2.21) ta có :
(2.22) Tích hai ma trận ở vế trái của phương trình (2.22) là một ma trận mà có thể được viết gọn lại bằng các ký hiệu sau:
Trong đó: f11 = cosΦ x + sinΦ y (2.23)
f12 = -sinΦ x + cosΦ y (2.24)
f13 = z (2.25)
và x, y, z là các phần tử của vectơ xác định bởi các dữ kiện f11, f12, f13, ví dụ: f11(n) = cosΦ nx + sinΦ ny
f12(O) = -sinΦ Ox + cosΦ Oy
f13(a) = az
Như vậy phương trình (3.22) có thể được viết thành :
(2.26) Trong đó f11, f12, f13 đã được định nghĩa ở (2.23), (2.24) và (2.25).
Khi tính toán vế trái, ta chú ý rằng px, py, pz bằng 0 vì phép biến đổi Euler chỉ toàn phép quay không chứa một phép biến đổi tịnh tiến nào, nên f11(p) = f12(p) = f13(p)= 0. Từ phương trình (2.26), cho cân bằng phần tử ở hàng 2 cột 3 ta có :
f12(a) = -sinΦ ax + cosΦ ay = 0. (2.27)
Cộng hai vế với sinΦ ax và chia cho cosΦ ax ta có :
Góc Φ có thể xác định bằng hàm arctg hai biến : Φ = arctg2(ay, ax)
Ta cũng có thể giải phương trình (2.27) bằng cách cộng hai vế với -cosΦ ay rồi chia hai vế cho -cosΦ ax, triệt tiêu -ax ở vế trái và cosΦ ở vế phải, ta có :
Trong trường hợp này góc Φ tìm được là : Φ = arctg2(-ay, -ax)
Như vậy phương trình (2.27) có một cặp nghiệm cách nhau 1800 (đây là nghiệm toán) và ta có thể viết :
Φ = arctg2(ay, ax) và Φ = Φ + 1800.
(Hiểu theo cách viết khi lập trình trên máy tính).
Nếu cả ax và ay đều bằng 0 thì góc Φ không xác định được. Điều đó xảy ra khi bàn tay chỉ thẳng lên trên hoặc xuống dưới và cả hai góc Φ và ψ tương ứng với cùng một phép quay. Điều này được coi là một phép suy biến (degeneracy), trong trường hợp này ta cho Φ = 0.
Với giá trị của Φ nhận được, các phần tử ma trận ở vế bên trái của phương trình (2.26) sẽ được xác định. Tiếp tục so sánh các phần tử của hai ma trận ta có :
f11(a) = cosΦ ax + sinΦ ay = sinθ Và f13(a) = az = cosθ.
Vậy θ = arctg2(cosΦ ax + sinΦ ay, az)
Khi cả hai hàm sin và cos đều được xác định như trường hợp trên, thì góc thường được xác định duy nhất và không xảy ra trường hợp suy biến như góc Φ trước đây. Cũng từ phương trình (2.26) ta có :
f12(n) = -sinΦ nx + cosΦ ny = sinψ f12(O) = -sinΦ Ox + cosΦ Oy = cosψ
Vậy : ψ = arctg2(-sinΦ nx + cosΦ ny, -sinΦ Ox + cosΦ Oy)
Tóm lại, nếu cho trước một phép biến đổi đồng nhất dưới dạng các phép quay, ta có thể xác định các góc Euler tương ứng là :
Φ = arctg2(ay, ax) và Φ = Φ + 1800
θ = arctg2(cosΦ ax + sinΦ ay, az)
ψ = arctg2(-sinΦ nx + cosΦ ny, -sinΦ Ox + cosΦ Oy)