2. Phương trình động học ngược:
2.2.Lời giải của phép biến đổi Euler và bài toán ứng dụng:
Phương trình Euler mô tả hướng của khâu chấp hành cuối:
Tập nghiệm muốn tìm là cá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ó :
(3.7) Ta có:
(3.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:
(3.9) (3.10) (3.11) (3.12)
(3.13) (3.14) (3.15) (3.16)
(3.17)
Ta thử giải hệ phương trình này để tìm Φ, θ, ψ như sau: Từ (3.17) ta có: (3.18) Từ (3.15) ta có: (3.19) Từ (3.11) và (3.18) ta có: (3.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.
- dcosθ/dθ|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 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 giá trị 0.
• Các phương trình (3.19) và (3.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 -π≤0<π.
Ví dụ:
• arrctang 2(-1/-1) = -1350
Hàm này xác định ngay cả khi x hoặc y bằng 0 và cho kết quả đúng.
Hình 3.10 – Hàm arctg2(y,x)
Để 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, Φ)-1, ta có:
(3.21) Vế trái của phương trình (3.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 (3.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ừ (3.21) ta có:
(3.22) Tích hai ma trận ở vế trải của phương trình (3.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 (3.23)
- f12 = -sinΦx + cosΦy (3.24)
Và x, y, x là các phần tử của vector xác định bởi các dữ kiện f11, f12, f13, ví dụ:
Như vậy phương trình (3.22) có thể được viết thành:
(3.26) 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 (3.26), cho cân bằng phần tử ở hàng 2 cột 3 ta có:
(3.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:
Ta cũng có thể giải phương trình (3.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à:
Như vậy phương trình (3.27) có một cặp nghiệm cách nhau 1800 (đây là nghiệm toán) và ta có thể viết:
(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 (3.27) 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ó:
Vậy:
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 (3.27) ta có:
Vậy:
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à: