CHƯƠNG 3: THIẾT KẾ BỘ ĐIỀU KHIỂN
3.3.2. Tính toán quan hệ vận tốc
Để xác định các thành phần động học của mobile robot ta giả sử đặt nó làm việc trong một mặt phẳng gắn liền với mặt đất như hình 3.1
Hình 3.2: Sơ đồ quan hệ vận tốc của robot
Gọi I là tâm quay tức thời của mobile robot trong khoảng thời gian , khoảng cách giữa hai chân là 2b. , là vận tốc chân bên trái, chân bên phải và vận tốc trọng tâm của mobile robot. Vận tốc góc của mobile robot quanh tâm I đước xác định như sau:
Từ các phương trình ta có:
Trị số thể hiện bán kính cung tròn mà mobile robot sẽ thực hiện với vận tốc . Để robot đi thẳng thì khi đó nghĩa là giá trị vận tốc của hai chân phải bằng nhau. Robot rẽ phải, rẽ trái hay di chuyển theo một cung tròn với bán kính bất kỳ ta chỉ cần thiết lập giá trị vận tốc cho phù hợp với các công thức trên.
Để tính toán vận tốc dài của legged mobile robot theo vận tóc góc động cơ mỗi chân, trước tiên ta khảo sát động học cơ cấu cơ cấu chân và xác định quỹ đạo di chuyển của bàn chân. Mục đích của phân tích này là tìm ra mối quan hệ giữa vận tốc dài và vận tốc góc của mỗi chân. Theo phương án cơ khí đặt ra robot có mười hai chân, quỹ đạo các chân chuyển động giống nhau về mặt động học nên ta phân tích rõ nét một chân rồi dựa vào đó tính toán chuyển động của robot. Thông số các khâu của cơ cấu chân robot dựa trên các nghiên cứu trước được thể hiện trong bảng sau [10]:
Đơn vị đo
[rad] [mm]
54 14 74 88 60 60 60 54 71 60
Bảng 3.1: Thông số các khâu của cơ cấu chân robot
Cơ cấu này lấy ý tưởng từ quá trình nghiên cứu về strandbeest của nhà điêu khắc Theo Jansen [11] mà kỹ sư Phạm Trường An đã hiện thực hóa thành cơ cấu chân robot di chuyển theo một quỹ đạo xác định trước. Nguyên lý hoạt động của cơ cấu như hình vẽ 3.3.
A B
C
E D
P
F H
θ1 θ2
θ6 θ3
θ4
θ5
θ33 θ44
θ1 l1 l2
l3
l4
l44 l5
l33
l66
l6
l7
ωn
Hình 3.3: Biểu diễn vòng vecto của mười hai khâu
Cơ cấu mười hai khâu được tạo bằng nhiều cơ cấu bốn khâu bản lề. Cơ cấu bốn khâu bản lề đầu tiên bao gồm các khâu ABCD trong đó khâu 1 là khâu giá và tay quay là thanh AB, cơ cấu bốn khâu bản lề thứ hai là ABHD.
Khâu 5, 6, 7 và 44 tạo thành chuỗi bốn thanh. Khâu 4, 5 và 8 tạo thành cơ cấu ba thanh quay quanh điểm D cố định, khâu 6, 66 và 9 tạo thành cơ cấu ba thanh khác , và quỹ đạo điểm P được xem như là chân robot , tên gọi các khâu và điểm cho trên hình 9. Tọa độ các điểm được xác định như sau:
Chọn điểm A đặt tại gốc tọa độ Oxy: A(0, 0) Vị trí điểm B:
Vị trí điểm C:
Vị trí điểm D:
Xét 4 khâu bản lề ABCD ta có thể viết phương trình vòng vecto của bốn khâu được xác định như sau:
⃗⃗ ⃗⃗⃗ ⃗⃗⃗ ⃗⃗⃗ (3.8) Chiếu lên các trục tọa độ Ox và Oy như sau:
(3.9) (3.10) Phương trình (3.4) và (3.5) có thể viết lại như sau:
Lấy tổng bình phương hai vế của phương trình ta được phương trình tương đương như sau:
Khai triển phương trình ta được
Rút gọn phương trình ta được phương trình sau:
Phương trình được đưa về dạng sau:
Với:
Để giải phương trình (3.6) ta đặt biến như sau:
Phương trình bậc hai trên có hai nghiệm:
√
√
[( √ ) ] Viết lại phương trình (3.4) và (3.5) ta được:
Lấy tổng bình phương hai vế của phương trình ta được phương trình tương đương như sau:
Sau khi rút gọn phương trình trên ta được phương trình:
Đưa phương trình trên về dạng (3.6):
Với:
[( √ ) ]
Ta có vuông có góc ̂ , ba thanh CD, DE và CE tạo thành liên kết cứng nên góc được xác định như sau:
Vị trí điểm E:
Vị trí điểm H:
Tương tự Xét 4 khâu bản lề ABHD ta có thể viết phương trình vòng vecto của bốn khâu được xác định như sau:
⃗⃗ ⃗⃗⃗ ⃗⃗⃗⃗⃗ ⃗⃗⃗⃗⃗
Chiếu lên các trục tọa độ Ox và Oy như sau:
(3.11) (3.12) Phương trình (3.7) và (3.8) có thể viết lại như sau:
(3.13)
(3.14) Lấy tổng bình phương hai vế của phương trình (3.9) và (3.10) ta được phương trình tương đương như sau:
Sau khi rút gọn ta được phương trình sau:
Ta đưa phương trình trên về dạng (3.6):
Với:
[( √ ) ] Tương tự từ phương trình (3.7) và (3.8) ta có:
(3.15)
(3.16) Lấy tổng bình phương hai vế của phương trình (3.11) và (3.12) ta được phương trình tương đương như sau:
Sau khi rút gọn phương trình trên ta được phương trình:
Với:
[( √ ) ] Xét bốn thanh DEFH khi biết giá trị chiều dài các cạnh và góc thì ta tìm được giá trị các góc . Phương trình vòng vecto của bốn khâu được xác định như sau:
⃗⃗⃗⃗⃗ ⃗⃗⃗ ⃗⃗⃗ ⃗⃗⃗
Chiếu lên trục tọa độ Oxy ta được phương trình sau:
(3.17)
(3.18) Ta có phương trình tương đương với (3.13) và (3.14)
(3.19) (3.20) Lấy tổng bình phương hai vế của phương trình (3.15) và (3.16) ta được phương trình tương đương như sau:
Sau khi rút gọn phương trình trên ta được phương trình:
Phương trình trên có dạng:
Với:
[( √ ) ] Vị trí điểm F:
Vị trí điểm P:
Sử dụng những kết quả này ta viết một đoạn code chạy trên phần mềm Matlab thì dễ dàng tìm được quỹ đạo điểm P khi góc thay đổi từ . Từ đó ta tìm được mối quan hệ gần đúng của điểm P và .
Kết quả mô phỏng quỹ đạo điểm P theo góc trên phần mềm Matlab:
Hình 3.4: Đồ thị quỹ đạo kết hợp ba chân robot
Từ dữ liệu từ Workspace matlab ta có tập hợp các tọa độ của và theo theta như hình vẽ.
Hình 3.5: Đồ thị hàm bậc hai và bậc ba của theta theo Px Phương trình bậc hai gần đúng:
Phương trình bậc ba gần đúng:
(3.21) Qua hình 3.5 ta thấy đồ thị hàm số bậc ba gần đúng với quỹ đạo chuyển động hơn hàm số bậc hai [21].
Giải phương trình trên ta được tập hợp nghiệm sau:
Vận tốc của điểm được thể hiện như sau:
̇ (3.22)
Quan hệ giữa theta và tọa độ :
Hình 3.6: Đồ thị hàm bậc nhất và bậc hai của theta theo Py Phương trình bậc hai gần đúng:
Phương trình bậc ba gần đúng:
Từ các đồ thị trên ta thấy, để chân robot tiếp xúc với mặt đất thì các giá trị phải là thấp nhất. Trên đồ thị tọa độ theo trục tung của bàn chân robot dao động (-125 đến -122) khoảng 3mm.
Hình 3.8: Quỹ đạo chuyển động ba chân robot theo trục hoành Theo đồ thị ta thấy khoảng dịch chuyển tức thời của một chân robot đi trên mặt đất liên tục là:
Cơ cấu chân của robot hoạt động bởi động cơ có vận tốc góc là omega.
Với n là số vòng quay của động cơ trong một phút. Xét tại thời điểm t có n = 1, ta xác định được khoảng dịch chuyển tức thời của robot như sau.
Vì ta điều khiển vị trí và vận tốc của legged mobile robot thông qua điều khiển hai động cơ kéo mỗi chân robot nên ta xem như vận tốc tức thời của mỗi chân robot bằng với vận tốc của bánh xe nào đó được tìm như sau:
Vậy vận tốc dài một chân của legged mobile robot gần bằng với vận tốc dài của một bánh xe có bán kính .
Mối quan hệ giữa vận tốc dài, vận tốc góc của legged mobile robot và vận tốc góc của bánh xe như sau:
* + [
] * + (3.23)