Để đảm báo quá trình hoạt động được trơn tru, mượt mà, thì vận tốc quay các động cơ trên các khớp cần phải có sự ổn định khi quá độ từ vận tốc hiện tại sang vận tốc kế tiếp. Việc này giúp cho Robot tránh được việc thay đổi vận tốc đột ngột gây ảnh hưởng xấu tới cơ cấu chấp hành, giảm thiểu được khả năng hỏng hóc và có thể chạy ổn định được trong thời gian dài. Quy hoạch qũy đạo chính là thuật toán giúp hệ thống thực hiện được điều này.
Nhóm sẽ sử dụng đặc tính của phương trình bậc 3 với biến là thời gian để áp dụng cho quá trình điều khiển. Phương trình quy hoạch quỹ đạo có dạng như sau:
(t ) =a0 +a1t +a2t2 +a3t3
Phương trình vận tốc có dạng như sau:
(t ) =a1 + 2 a2t + 3a3t2
Đồ thị phương trình chuyển động của góc sẽ có dạng như sau:
Hình 2.4:Đồ thị góc theo thời gian.
Với 0 là vị trí ban đầu của khớp, f là vị trí mong muốn, t f − t0 là thời gian đáp ứng mong muốn.
Để có thể tìm được các hệ số cho phương trình quy hoạch quỹ đạo, trước hết ta cần phải xác định được vị trí và vận tốc của hiện tại là 0 , 0 và của điểm mong muốn là f ,f . Chọn thời điểm bắt đầu làt0 = 0 . Từ đó xây dựng được một hệ phương
trình như sau:
f
0
Từ phương trình trên, ta tính ra được các hệ số cho phương trình quy hoạch quỹ đạo là:
BỘ MÔN TỰ ĐỘNG ĐIỀU KHIỂN
CHƯƠNG 2. CƠ SỞ LÝ THUYẾT a0 a1 a2 a3 =0 =0 = 3( f − 0 ) − 20 − 1 f t2 ft ft f = −2( f − 0 ) + 1( f − 0 ) t3 ft2 f
Thay các hệ số vừa tính được vào phương trình quy hoạch quỹ đạo, ta có được phương trình (t ) =f (0 ,0 ,f , f , t) . Áp dụng phương trình này cho quá trình tính toán các hoạt động di chuyển của Robot có thể giúp đảm bảo được các tiêu chí như đã nêu ra ban đầu.
Trong mô hình, nhóm dự định sử dụng động cơ AC Servo cho Robot. Động cơ AC Servo sử dụng nguyên lý điều khiển là nhận xung ngõ vào làm tín hiệu điều khiển, dự vào cài đặt số xung cho mỗi vòng quay để tính vị trí quay của động cơ. Từ công thức quy hoạch quỹ đạo trên, nhóm có được giá trị góc quay theo thời gian. Để có thể điều khiển động cơ quay theo giá trị góc này, nhóm tính toán số lượng xung phát ra để điều khiển động cơ theo công thức sau:
P =( d
−
ht ).PPR
360
Với d là góc đặt, ht là góc hiện tại, PPR là tổng số xung cần để quay hết một vòng của trục quay.
2.3. Phương pháp nhận diện vị trí của vật trong không gian
Việc nhận diện vật cần thông số đầu vào từ camera phải chính xác. Các camera hiện nay thường không tránh khỏi lỗi khi chế tạo thấu kính hội tụ, dẫn đến việc hình ảnh đi vào các cảm biến ảnh có một độ méo ảnh nhất định. Để có thể xác định được các hệ số ảnh hưởng đó và tiêu cự thực tế của máy ảnh, ta cần xác định mối quan hệ giữa các điểm trong không gian thực và hình chiếu 2D của điểm đó trong hình ảnh được chụp bởi máy ảnh đó [9].
Bên cạnh các thông số nội thì độ chính xác của các thông số ngoại như hướng và vị trí của Camera cũng góp phần tạo nên kết quả tốt hơn cho việc nhận diện. Nhóm sử dụng công cụ của OpenCV để tiến hành ước lượng các thông số nội và thông số ngoại của camera [4]. Đầu tiên là chụp các hình ảnh bàn cờ với các vị trí và các góc trong không gian như hình 4.1, sau đó tiến hành chạy thuật toán của thư viện để ước
BỘ MÔN TỰ ĐỘNG ĐIỀU KHIỂN
lượng. Cuối cùng sử dụng các thông số vừa ước lượng được để hiệu chỉnh ra một các bức hình với độ chuẩn xác hơn, giảm thiểu hiện tượng méo góc, méo ảnh.
Hình 2.5:Các ảnh chụp mẫu dùng cho quá trình hiệu chỉnh camera.
Hình 2.6:a Chưa hoàn thiện. b Đã hiệu chỉnh.
Có thể thấy hình ảnh thu được trực tiếp từ camera có đường kẻ đỏ lệnh hơn so với hàng gạch trong thực tế. Hình đã qua hiệu chỉnh cho ra đường kẻ đỏ gần như trùng với hàng gạch. Qua đó cho thấy hiệu chỉnh ảnh đã có tác dụng làm phẳng ảnh, và sát với tỉ lệ thực tế.
Tiếp theo, nhóm sử dụng kết hợp 2 camera để có thể nhận dạng vị trí của vật trong không gian 3 chiều. Đối với một vật thể trong không gian thì ta sẽ luôn thu được 2 tọa độ điểm ảnh tương ứng trên màn ảnh của mỗi camera, cùng với các thông số về
BỘ MÔN TỰ ĐỘNG ĐIỀU KHIỂN
CHƯƠNG 2. CƠ SỞ LÝ THUYẾT
vị trí của camera có thể đo đạc được thì nhóm biểu diễn mô hình một hệ hai camera như hình sau.
Hình 2.7:Mô hình không gian của hệ 2 camera
Trong hình trên, vị trí của vật cần nhận diện trong hệ trục tọa độ gốc là điểm
A( x0 , y0 , z0 ) , vị trí chiều quay của hai camera được đặt trước lần lượt là C1 có hướng chiếu theoZ1 ,C2 có hướng chiếu theoZ2 . Hình ảnh của vật được ghi lại trên khung
hình của hai camera tương ứng với 2 điểm P1
khung hình Camera 2. Hai điểm này có tọa độ
với hệ trục tọa độ của 2 camera. Điểm B là hình chiếu của điểm P2
tọa độ (u2 , 0, f2 ) .
Đầu tiên, ta cần chuyển các điểm P , P , B về hệ trục tọa độ gốc chứa điểm A
1 2
(chính là hệ trục thứ 0 của Robot). Với Camera 1, ta có được vị trí trong hệ trục tọa độ gốc là C1 ( x1 , y1 , z1 ) và các góc xoay tương ứng ( 1 , 1 , 1 ) , từ đó tính ra được ma trận chuyển đổi đồng nhất của Camera 1 là:
C01T = C1 Với: 0 C1 1 R = 0 0
Từ ma trận trên, ta tính được tọa độ của P1 trong hệ trục gốc theo công thức sau:
BỘ MÔN TỰ ĐỘNG ĐIỀU KHIỂN
Làm tương tự với Camera 2 thì ta có được tọa độ tương ứng của hai điểm P2 , B
trong hệ trục tọa độ gốc.
Như đã thể hiện trong hình 2.7 thì điểm A là giao điểm của 2 đường thẳng C1
P1 và C2 P2 . Như vậy, ta sẽ đưa về bài toán tìm giao điểm của 2 đường thẳng trong không gian 3 chiều. Nhưng có một điều ảnh hưởng là các thông số đầu vào để tính 2 đường thẳng này phải tuyệt đối chính xác nếu không chúng sẽ không giao nhau. Để giải quyết vấn đề trên, nhóm chuyển sang dạng bài toán tìm giao điểm của đường thẳng với mặt phẳng, như vậy dù có chút sai số thì bài toán vẫn cho ra nghiệm. Mặt phẳng được chọn ở đây chính là mặt phẳng tạo bởi 3 điểm P2 , C2 , B và đường thẳng được chọn sẽ tạo bởi 2 điểm P1 , C1 .
Ta lập phương trình mặt phẳng và phương trình đường thẳng trong không gian Oxyzdựa trên công thức tổng quát lần lượt như sau:
ptmp : ax +by +cz +d = 0 (với a, b, c, d là các hằng số)
x =m +e * t z =k +i * t
(với m, n, k, e, i, j là các hằng số)
Tiếp theo, thế x, y, z của phương trình đường thẳng vào phương trình mặt phẳng, ta được 1 phương trình với một ẩn duy nhất là t. Giải phương trình tìm tìm ra được ẩn t. Thay t vào phương trình đường thẳng sẽ có được tọa độ điểm A.
2.4. Khả năng tránh vật cản của Robot.
Để đơn giản bài toán, nhóm tiến hành thiết lập một khoảng cách an toàn của một số điểm trên thân Robot đối với một điểm duy nhất trong không gian. Khoảng cách an toàn này được đặt là od, tọa độ của vật cản là Po . Đo khoảng cách từ Po
đến Robot theo công thức sau:
dok =0Pk −Po
Với 0
Pk ( k n) tọa độ các điểm được chọn trên thân Robot. Chọn càng nhiều điểm thì khả năng né càng hiệu quả nhưng các phép toán sẽ càng phức tạp. Vì vậy nên cân nhắc việc chọn lựa số lượng điểm phù hợp.
BỘ MÔN TỰ ĐỘNG ĐIỀU KHIỂN
CHƯƠNG 2. CƠ SỞ LÝ THUYẾT
Trong thực tế thì vật cản sẽ là một khối, tức là một hệ các tọa độ của vật trong không gian. Vì vậy ta có thể tính toán thật nhiều các khoảng cách từ khối vật đến điểm trên thân Robot nếu cần thiết cho việc né tránh tốt hơn.
dok
Với m là số lượng tọa độ điểm tạo nên khối vật cản.
Lúc này mục tiêu của hệ thống sẽ bao gồm giảm thiểu sai số về vị trí, hướng và giữ khoảng cách an toàn với vật cản. Hàm mục tiêu sẽ có thêm thành phần tránh né vật theo phương trình sau:
− o ( doT do −od 2 )
E =E + e k k
1
k
Trong đó: o là hằng số dương tự chọn, hàm mũ ở đây sẽ có vai trò tạo ra vận tốc đẩy Robot ra xa vật cản khi Robot nằm trong khoảng an toàn.
Vận tốc góc mới sẽ là:
2.5. Các phần mềm hỗ trợ.