Để xõy dựng mụ hỡnh động lực học robot dựng phương trỡnh Lagrange bậc hai , cần biết vận tốc của điểm bất kỡ trờn tay mỏy .
: gúc quay của khớp quay thứ i (i = 1 - 5 ) : khối lượng của thanh nối thứ i
: moment quỏn tớnh của thanh thứ i đối với trục qua tõm khối của thanh nối : chiều dài từ khớp i đến tõm khối thanh nối i
Theo kết quả tớnh toỏn của phần trước , ta đó cú toạ độ của trọng tõm cỏc thanh nối như sau :
C1
- tọa độ của trọng tõm thanh nối thứ hai
C2 - tọa
độ của trọng tõm thanh nối thứ ba C3
- tọa độ của trọng tõm thanh nối thứ tư C4
- tọa độ của trọng tõm thanh nối thứ năm C5
Từ cỏc toạ độ trờn , ta dễ dàng tớnh được bỡnh phương tốc độ dài của tõm khối cỏc thanh nối từ 1 đến 5 dựa vào cụng thức sau :
Ta cú kết quả sau :
-Tốc độ dài của thanh nối thứ nhất
-Tốc độ dài của thanh nối thứ ba
-Tốc độ dài của thanh nối thứ tư
-Tốc độ dài của thanh nối thứ năm
Do đú ta tớnh được động năng của tõm khối thanh nối từ 1 đến 5 theo cụng thức sau :
Trong đú mi ,Vi, i lần lượt là khối lượng , vận tốc và gúc quay của thanh nối thứ i với i =(1,2…5);
Và Ji là Jacobien của thanh nối thứ i được tớnh bởi cụng thức
Do robot chỉ hoạt động trong mặt phẳng ngang 2-d (xy). Phương trỡnh Lagrange được đơn giản thành
= ;
Trong đú : Ki : động năng , Pi : thỊ năng của thanh nối i;
và với i =1,2…5
Từ đú ta cú thể tớnh momen của khớp thứ i theo cụng thức
= -
Sau khi rỳt gọn , ta được phương trỡnh động lực học của robot cú dạng như sau :
Trong đú ma trận H là ma trận vuụng 5 hàng , 5 cột và ma trận V là ma trận 5 hàng , 1 cột .
Trong đú Hii và Vi cú cỏc biểu thức cụ thể nh sau : + Ma trận H
- 5 giỏ trị của hàng thứ hai
- 5 giỏ trị của hàng thứ ba ;
;
- 5 giỏ trị của hàng thứ tư ;
; ;
Chương 3