CHƯƠNG 2: CƠ SỞ LÝ THUYẾT 2.1. Tính toán động học thuận và ngƣợc rôbốt dƣ dẫn động bằng thuật toán hiệu chỉnh gia lƣợng vectơ tọa độ suy rộng
2.1.4. Giải bài toán động học ngược rôbốt dư dẫn động bằng phương pháp hiệu chỉnh gia lượng vectơ tọa độ suy rộng
2.1.4.2. Các công thức xác định vectơ vận tốc và vectơ gia tốc suy rộng 31 2.1.4.3.Các công thức xác định vectơ tọa độ suy rộng
Ký hiệu là vectơ tọa độ suy rộng, là vectơ xác định vị trí và hướng của bàn kẹp trong hệ quy chiếu cố định. Từ bài toán động học thuận ta có hệ thức:
x=f(q) (2.31)
Trong điều khiển rôbốt chúng ta thường gặp yêu cầu của bài toán công nghệ là phải điều khiển bàn kẹp chuyển động theo 1 quỹ đạo mong muốn cho trước nào đó, nghĩa là chúng ta phải xác định các giá trị của q để tạo ra giá trị mong muốn cho x.
Như vậy ở bài toán động học ngược chúng ta phải thiết lập được quan hệ:
(2.32)
Trước hết đạo hàm hai vế của (2.31) theo thời gian, ta được:
(2.33)
Trong đó J(q) là ma trận Jacobi cỡ mxn, với:
(2.34)
Giả sử hạng của ma trận J(q) là m, vậy ta chọn ma trận tựa nghịch đảo của ma trận chữ nhật J(q) dưới dạng:
(2.35)
Khi đó từ biểu thức (2.33) ta suy ra công thức tính vectơ vận tốc suy rộng:
(2.36)
Như thế nếu biết được x(t), ta tìm được , biết được hàm f(q(t)) ta tìm được . Theo (2.36) ta tính được . Vấn đề ở đây là tìm cách xác định q(t). Vấn đề này sẽ được trình bày ở mục tiếp theo.
Đạo hàm 2 vế của (2.36) theo thời gian, ta được công thức xác định vectơ gia tốc suy rộng:
(2.37) Để có thể áp dụng được công thức (2.37) ta cần phải tính được . Theo lý thuyết ta có thể đạo hàm ma trận theo thời gian. Tuy nhiên rất khó xác định biểu thức giải tích của ma trận này, do đó ta phải tìm một thuật toán xác định . từ biểu thức (2.35) ta suy ra:
(2.38) Đạo hàm 2 vế của (2.38) theo thời gian, ta được:
(2.39) Từ (2.39) ta suy ra:
(2.40) Ma trận được tính bằng cách đạo hàm trực tiếp các phần tử của ma trận theo thời gian. Thế biểu thức (2.40) vào (2.37) ta tìm được gia tốc .
2.1.4.3. Các công thức xác định vectơ tọa độ suy rộng:
Sử dụng các công thức (2.36), (2.37) và (2.40) ta sẽ xác định được và , nếu như đã xác định được q(t) và x(t) tại thời điểm khảo sát. Như vậy, để xác định và trước hết ta cần xác định q(t). Thuật toán xác định q(t) như sau:
Chia khoảng thời gian làm việc của rôbốt [0,T] thành N khoảng bằng nhau:
, ta có với k=0, 1,..., N-1.
Áp dụng khai triển Taylor đối với quanh giá trị , ta được:
(2.41)
Thế biểu thức (2.36) vào (2.41) và bỏ qua các vô cùng bé bậc 2 ta được:
với k=0, 1,..., N-1 (2.42) Từ đó, ta có các bước tính toán như sau:
1. Tìm q0.
2. Tính .
3. Tính theo (2.36) và tính theo (2.37).
4. Tính theo (2.42), rồi tính theo (2.36) và (2.37).
Ta thấy việc tính theo (2.42) là khá thô. Vì vậy để nâng cao độ chính xác, ta cần có một thuật toán xác định chính xác hơn dưới đây.
2.1.4.3.1. Hiệu chỉnh gia lƣợng vectơ tọa độ suy rộng tại thời điểm t0: Giả sử t0=0 là thời điểm đầu. Ta có thể xác định giá trị gần đúng của q0 bằng cách vẽ (hoặc thực nghiệm). Sau đó áp dụng khai triển Taylor để tìm gần đúng tốt hơn của q0 như sau:
Giả sử lấy . Theo phương trình (2.31), ta có:
Từ đó suy ra:
(2.43)
Giải phương trình đại số tuyến tính (2.43) với ẩn , ta được:
Nếu thì ta tính lại giá trị mới của bởi rồi thế vào (2.43) và lại giải phương trình này. Quá trình lặp được dừng lại khi .
Và khi đó ta gán giá trị tìm được là q0.
(2.44)
Biết được nghiệm q0 của phương trình (2.29) tại thời điểm t0=0. Thay vào (2.36), (2.37) ta tìm được và .
Giá trị sai số cho phép được xác định theo yêu cầu về độ chính xác của bài toán.
2.1.4.3.2. Hiệu chỉnh gia lƣợng vectơ tọa độ suy rộng tại thời điểm tk+1:
Trước tiên ta xác định giá trị gần đúng theo công thức (2.42):
(2.45)
Sau đó ta tìm biểu thức gần đúng tốt hơn của :
(2.46)
Ta cần xác định :
Thế (2.46) vào phương trình (2.31), ta được:
Từ đó suy ra:
(2.47) Chú ý rằng là ma trận cỡ mxn, là vectơ có n phần tử. Do đó số ẩn lớn hơn số phương trình (m<n), nghiệm có chuẩn nhỏ nhất của phương trình (2.47) có dạng:
(2.48) Sau đó lấy:
k 1 k 1 k 1 (2.49)
Nếu k 1 thì thay (2.47) vào (2.48) và tiếp tục giải phương trình (2.47).
Nếu k 1 thì ta lấy:
k 1 k 1 (2.50)
Biết được k 1, ta tính k 1 theo công thức (2.36) và k 1 theo công thức (2.37).
Trên hình 2.4 trình bày sơ đồ khối giải bài toán động học ngược rôbốt dư dẫn động bằng phương pháp hiệu chỉnh gia lượng vectơ tọa độ suy rộng.
k=0
sai
Hình 2.4: Sơ đồ khối giải bài toán động học ngược k=k+1
KẾT THÚC
Đúng
Xuất kết quả qk
đúng
sai