Hình 2.4. Khối Dymamic Model
Ngõ vào của khối này là các mômen (2.11) và các lực (2.12). Ngõ ra sẽ là tọa độ tổng quát (2.1) và đạo hàm của tọa độ tổng quát. Trong khối này có hai khối con có chức năng quan trọng:
Khối A(q): tính các phần tử của ma trận quán tính A trong phương trình (2.10). Khối b(q,pq,M,F): tính các phần tử của vector b trong phương trình (2.10).
Khối A(q):
Hình 2.5. Khối Ma trận A(q)
Khối ma trận A(q) ở hình 2.5 bao gồm 7 cột, tương ứng với 7 cột trong ma trận quán tính A(q) của phương trình động học (2.10).
Cột thứ 1: Chứa các hệ số . Các hệ số này sẽ được chứa bởi các khối hàm Fcn f(u) như hình 2.6 bên dưới. Các hệ số trong cột này được trình bày ở phụ lục A.1.1.
Hình 2.6. Cột thứ nhất trong ma trận A(q)
Cột thứ 2: Chứa các hệ số . Các hệ số này sẽ được chứa bởi các khối hàm Fcn f(u) như hình 2.7 bên dưới. Các hệ số trong cột này được trình bày ở phụ lục A.1.2.
Cột thứ 3: Chứa các hệ số . Các hệ số này sẽ được chứa bởi các khối hàm Fcn f(u) như hình 2.8 sau đây. Các hệ số trong cột này được trình bày ở phụ lục A.1.3.
Hình 2.8. Cột thứ ba trong ma trận A(q)
Cột thứ 4: Chứa các hệ số . Các hệ số này sẽ được chứa bởi các khối hàm Fcn f(u) như hình 2.9. Các hệ số trong cột này được trình bày ở phụ lục A.1.4.
Cột thứ 5: Chứa các hệ số . Các hệ số này sẽ được chứa bởi các khối hàm Fcn f(u) như hình 2.10. Các hệ số trong cột này được trình bày ở phụ lục A.1.5.
Hình 2.10. Cột thứ năm trong ma trận A(q)
Cột thứ 6: Chứa các hệ số . Các hệ số này sẽ được chứa bởi các khối hàm Fcn f(u) như hình 2.11 bên dưới. Các hệ số trong cột này được trình bày ở phụ lục A.1.6.
Cột thứ 7: Chứa các hệ số . Các hệ số này sẽ được chứa bởi các khối hàm Fcn f(u) như hình 2.12 bên dưới. Các hệ số trong cột này được trình bày ở phụ lục A.1.7.
Hình 2.12. Cột thứ bảy trong ma trận A(q)
Khối b(q,pq,M,F):
Khối này chứa các phần tử của vector ( ̇ ) (hình 2.13). Các hệ số của vector được trình bày ở phụ lục A.2.
2.3.2.2. Khối Ground contact:
Khối Ground contact có nhiệm vụ phát hiện chân chạm đất và xác định các ngoại lực tác động lên bàn chân của robot. Khối này cũng có nhiệm vụ nhận ra lực đang tác động lên chân trái, chân phải hay cả hai chân, từ đó tính toán và đưa ra giá trị đầu vào cho khối Dynamic model.
Hình 2.14. Khối Ground contact
Tín hiệu vào của khối là các tọa độ tổng quát (2.1) và đạo hàm của các tọa độ tổng quát. Đạo hàm các tọa độ sẽ cho ra vận tốc di chuyển của đầu chân. Từ đó, dùng các giá trị đưa vào để tính toán lực tác động lên đầu chân trái, lực tác động lên đầu chân phải. Đầu ra của khối sẽ là lực tổng cộng, trạng thái chân, cảm biến trạng thái chân.
Khối Ground contact / Leg tip position
( ) ( ( )) ( ( ) ( )) ( ( ) ( ) ( )) ( ) ( ( )) ( ( ) ( )) ( ( ) ( ) ( ))
( ) ( ( )) ( ( ) ( )) ( ( ) ( ) ( )) ( ) ( ( )) ( ( ) ( )) ( ( ) ( ) ( ))
Đầu vào của khối này sẽ là các tọa độ tổng quát, đầu ra sẽ là tọa độ của hai đầu chân . Các khối hàm Fcn sẽ chứa các biểu thức tính toán cho
.
Khối Ground contact / Leg tip speed
Hình 2.16. Khối Leg tip speed
( ) ( ( )) ( ) ( ( ) ( ))( ( ) ( )) ( ( ) ( ) ( ))( ( ) ( ) ( )) ( ) ( ( )) ( ) ( ( ) ( ))( ( ) ( )) ( ( ) ( ) ( ))( ( ) ( ) ( )) ( ) ( ( )) ( ) ( ( ) ( ))( ( ) ( )) ( ( ) ( ) ( ))( ( ) ( ) ( )) ( ) ( ( )) ( ) ( ( ) ( ))( ( ) ( )) ( ( ) ( ) ( ))( ( ) ( ) ( ))
Đầu vào của khối này là giá trị đạo hàm của tọa độ tổng quát. Đầu ra là tốc độ của các đầu chân trái và phải, được thể hiện qua đạo hàm của tọa độ chân. Cũng
tương tự như khối trên, các khối hàm Fcn sẽ chứa các biểu thức tính toán cho tốc độ đầu chân trái và phải.
Khối Ground contact / Calculate forces
Hình 2.17. Khối Calculate forces
Trong khối này, đầu tiên các tọa độ của đầu ngón chân sẽ được chiếu xuống hệ tọa độ cục bộ ( ) nhờ khối Leg tip projection, sau đó sẽ tính toán các lực trực giao và lực tiếp tuyến nhờ khối Control và cuối cùng chiếu xuống các lực tác động nhờ khối Force projection. Các giá trị cảm biến và trạng thái khởi tạo cũng được đưa ra ngoài.
Ground contact / Calculate forces / Leg tip projection
Để tính toán việc chiếu tọa độ, các vector tọa độ đơn vị của hệ tọa độ ( ) đầu tiên được xác định nhờ khối Vectors. Trong hai khối Projection giống nhau, điểm tham chiếu cho trước được trừ từ các vector đầu vào (vị trí và tọa độ của đầu ngón chân), sau đó việc chiếu được thực thi bằng cách sử dụng tích chập. Biến ground chứa các điểm đất được cho từ bảng tham số của robot.
Hình 2.18. Khối Leg tip projection
Ground contact / Calculate forces / Leg tip projection / Vectors
Hình 2.19. Khối Vectors
Khối này có nhiệm vụ xác định các vector tọa độ đơn vị để tính toán các vector đơn vị trực giao, vector đơn vị tiếp tuyến và điểm tham chiếu. Khối Ground contact / Calculate forces / Controls
Khối Controls chứa các khối control cho các lực trực giao và lực tiếp tuyến. Giá trị đầu vào cho khối này là vị trí và tốc độ của đầu ngón chân trong hệ tọa độ ( ) và giá trị ngõ ra là các giá trị cảm biến tiếp xúc, các lực trực giao và lực tiếp tuyến cũng như biến trạng thái gcstate.
Khối Ground contact / Calculate forces / Controls / Control y’g
Hình 2.21. Khối Control y’g
Lực trực giao được tính toán khi vị trí đầu ngón chân dưới mức zero. Giá trị ngõ ra của khối PD sẽ tương ứng như công thức :
̇
Trong đó là vị trí tung độ của đầu ngón chân trong tọa độ , là hệ số đàn hồi và là tỉ số tắt dần chuẩn.
Giá trị này được giới hạn trong giá trị dương. Nếu không được bật, giá trị ra của khối PD sẽ là zero.
Hình 2.22. Khối Control x’g
Khối Control x‟g lưu giữ vị trí khởi tạo của đầu ngón chân hoặc trong hệ thống con Update initial position được bật. Giá trị được cập nhật khi chân đầu tiên chạm đất hoặc chân đang trượt đi trong bước đi mô phỏng trước đó. Các biến gcstate định nghĩa các trạng thái khởi tạo của đầu ra ( ) của khối con và khối Memory (đang trượt trái, đang trượt phải).
KhốiGround contact / Calculate forces / Controls / Control x’g/ Control
Khối Control này quyết định rằng một chân nào đó có trượt hay không và tính toán lực điều khiển tiếp tuyến tương ứng. Nếu chân không trượt, lực là ngõ ra của khối PD và tương ứng theo công thức:
( ) ̇
Trong đó và là các thuộc tính tiếp tuyến đất. Khi chân trượt, lực sẽ được tính toán theo công thức:
Trong đó là hệ số ma sát khi chuyển động.
Khối Ground contact / Calculate forces / Force projection
Hình 2.24. Khối Force projection
Khối này chiếu các lực trực giao và lực tiếp tuyến tác động lên đầu ngón chân thành các ngoại lực. Đầu vào Ground vectors chứa các vector đơn vị trực giao và tiếp tuyến đất.
2.3.2.3. Khối Knee stopper:
Khối này có nhiệm vụ cộng thêm giá trị mômen vào các giá trị mômen của khớp nối đầu gối nếu góc của khớp nối này không nằm trong khoảng giới hạn mong muốn. Cả hai đầu gối đều được điều khiển bằng cùng một khối Knee control.
Khối Knee stopper / Knee control (L)
Hình 2.26. Khối Knee control (L)
Khoảng giới hạn của góc đầu gối được cho được nhớ trong hai biến kneemin và kneemax. Nếu giá trị của góc đầu gối bắt đầu thấp hơn giá trị giới hạn dưới, khối điều khiển PD[8] sẽ cộng thêm các giá trị mômen giới hạn, nhưng các giá trị này phải là giá trị dương. Tương tự, nếu giá trị của góc đầu gối bắt đầu vượt ngưỡng giá trị trên, khối PD sẽ thêm vào các giá trị mômen, nhưng là giá trị âm.
CHƢƠNG 3
ĐIỀU KHIỂN ROBOT
ĐI BẰNG HAI CHÂN BẰNG
PHƢƠNG PHÁP ĐIỀU KHIỂN PD