Đang tải... (xem toàn văn)
Điều khiển omni robot sử dụng thuật toán DSC và mô phỏng bằng Sfunction. aaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaa Điều khiển omni robot sử dụng thuật toán DSC và mô phỏng bằng Sfunction. aaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaa Điều khiển omni robot sử dụng thuật toán DSC và mô phỏng bằng Sfunction. aaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaa Điều khiển omni robot sử dụng thuật toán DSC và mô phỏng bằng Sfunction. aaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaa Điều khiển omni robot sử dụng thuật toán DSC và mô phỏng bằng Sfunction. aaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaa Điều khiển omni robot sử dụng thuật toán DSC và mô phỏng bằng Sfunction. aaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaa Điều khiển omni robot sử dụng thuật toán DSC và mô phỏng bằng Sfunction. aaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaa Điều khiển omni robot sử dụng thuật toán DSC và mô phỏng bằng Sfunction. aaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaa Điều khiển omni robot sử dụng thuật toán DSC và mô phỏng bằng Sfunction. aaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaa Điều khiển omni robot sử dụng thuật toán DSC và mô phỏng bằng Sfunction. aaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaa Điều khiển omni robot sử dụng thuật toán DSC và mô phỏng bằng Sfunction. aaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaa
Thiết kế điều khiển cho xe tự hành sử dụng thuật toán DSC LỜI NÓI ĐẦU Cùng với phát triển xã hội loài người , ngành khoa học kỹ thuật không ngừng đến thành công Nhiều công trình khoa học, phát minh nhà khoa học vào sống, phục vụ lợi ích người Ngày Robot nghiên cứu ứng dụng rộng rãi ngành công nghiệp, thay cho hoạt động người môi trường độc hại, nguy hiểm Nổi bật số robot tự hành Là robot ứng dụng rộng rãi đời sống công nghiệp Trong thời gian thực tập trường cao đẳng nghề công nghệ cao HHT, hướng dẫn bảo tận tình GS.Phan Xuân Minh TS.Ngô Mạnh Tiến chúng em tìm hiểu nguyên lý hoạt động bánh xe Omni mô hình động học động lực học robot tự hành Từ đưa thuật toán điều khiển theo nguyên lý MSS hay DSC cho robot Tuy nhiên báo cáo chúng em nhiều sai sót Vì chúng em mong có góp ý, phê bình thầy cô để báo cáo hoàn thiện Chúng em xin chân thành cám ơn ! Nguyễn Quế Hà – Trần Dục Tú – ĐKTĐH –K56 Thiết kế điều khiển cho xe tự hành sử dụng thuật toán DSC MỤC LỤC LỜI NÓI ĐẦU MỤC LỤC .2 DANH MỤC HÌNH VẼ ĐẶT VẤN ĐỀ I Cấu trúc xe tự hành (Omni Robot) Xe tự hành ? .6 Các phận xe Bánh xe Omni .6 Cách di chuyển bánh xe omni .8 Sơ đồ vector biểu diễn chiều di chuyển robot sử dụng bánh xe omni Thân xe 10 II Mô hình hóa xe tự hành 11 Mô hình động học 11 Mô hình động lực học 12 III Thiết kế điều khiển cho robot omni bánh 13 Phương pháp Multiple Sliding Surface Control (MSS) 13 Lý thuyết chung .13 Áp dụng cho đối tượng Omni robot 14 Phương pháp DSC ( Dynamic Surface Control ) .15 Lý thuyết chung .15 Áp dụng cho đối tượng Omni robot 15 Mô Matlab ( Sử dụng công cụ S-function ) 17 Theo Backstepping 17 Theo DSC 20 Code lập trình Sfunction 22 Khối tạo quỹ đạo tròn 22 Khối tạo quỹ đạo thẳng .26 Khối BackStepping 31 Khối DSC 36 Mô hình Robot 41 PHỤ LỤC 47 Mô hình động lực học 47 Nguyễn Quế Hà – Trần Dục Tú – ĐKTĐH –K56 Thiết kế điều khiển cho xe tự hành sử dụng thuật toán DSC Mô hình động lực học 52 So sánh mô hình động học báo nước với nước 57 Ở báo nước 57 Ở Việt Nam .58 Thiết kế điều khiển 59 Điểm ổn định .59 Theo dõi quỹ đạo .60 Dead – Reckoning .61 TÀI LIỆU THAM KHẢO .62 Nguyễn Quế Hà – Trần Dục Tú – ĐKTĐH –K56 Thiết kế điều khiển cho xe tự hành sử dụng thuật toán DSC DANH MỤC HÌNH VẼ Hình : Cấu trúc đơn giản xe tự hành Hình : Một số loại bánh xe omni Hình : Khả di chuyển robot omni Hình : Mô tả cách di chuyển omni robot Hình :Hình ảnh bánh xe Mecanum Hình : Sơ đồ vector chuyển động tiến Hình : Sơ đồ vector chuyển động lùi Hình : Sơ đồ vector chuyển động sang trái Hình : Sơ đồ vector chuyển động sang phải Hình 10 :Sơ đồ vector mô tả hướng di chuyển robot omni bốn bánh Hình 11 : Sơ đồ mạch điện tử điều khiển động xe tự hành Hình 12 : Sơ đồ khối mô hình điều khiển xe tự hành Hình 13 : Mô hình robot omni bánh Hình 14 : Sơ đồ Simulink quỹ đạo đặt quỹ đạo thẳng theo Backstepping Hình 15 : Robot chạy bám quỹ đạo thẳng theo Backstepping Hình 16 : Đồ thị sai lệch vị trí quỹ đạo đặt quỹ đạo thẳng theo Backstepping Hình 17 : Sơ đồ Simulink quỹ đạo đặt quỹ đạo tròn theo Backstepping Hình 18 : Robot bám quỹ đạo tròn theo Backstepping Hình 19 : Sai lệch quỹ đạo quỹ đạo đặt quỹ đạo thẳng theo Backstepping Hình 20 : Sơ đồ Simulink quỹ đạo đặt quỹ đạo thẳng theo DSC Hình 21 : Robot chạy bám theo quỹ đạo thẳng theo thuật toán DSC Hình 22 : Sai lệch quỹ đạo quỹ đạo đặt quỹ đạo tròn theo thuật toán DSC Hình 23 : Sơ đồ Simulink quỹ đạo đặt quỹ đạo tròn theo DSC Hình 24 : Robot bám quỹ đạo tròn theo DSC Hình 25 : Sai lệch quỹ đạo quỹ đạo đặt quỹ đạo tròn theo DSC Hình 26 : Mô tả hướng di chuyển vận tốc bánh robot omni bánh Hình 27 : Hệ tọa độ cục robot omni bánh Hình 28 : Giản đồ di chuyển bánh Hình 29 : Mô tả vận tốc bánh, góc lệch vận tốc góc robot Hình 30 : Chiếu thành phần vecto vận tốc tiếp tuyến với bánh xe lên trục tọa độ x y Hình 31 : Mô tả vận tốc dài góc lệch bánh xe so với khung tọa độ robot Hình 32 : Mô tả lực kéo chiều vận tốc góc lệch bánh xe so với khung tọa độ robot Hình 33 : Momen lực tác động lên robot Hình 34 : Mô hình robot tự hành chiều dương vận tốc góc,momen lực tác động lên robot Hình 35 : Mô tả robot Omni bốn bánh theo tài liệu nước Hình 36 : Mô tả vận tốc dài bốn bánh xe - vận tốc dài pháp tuyến robot Nguyễn Quế Hà – Trần Dục Tú – ĐKTĐH –K56 Thiết kế điều khiển cho xe tự hành sử dụng thuật toán DSC ĐẶT VẤN ĐỀ Robot tự hành (Mobile Robot) thành phần có vai trò quan trọng ngành Robot học Cũng với phát triển mạnh mẽ hệ thống Cơ - Điện tử, robot tự hành ngày hoàn thiện cho thấy lợi ích công nghiệp sinh hoạt Một vấn đề quan tâm nghiên cứu robot tự hành làm để robot di chuyển tới vị trí xác định mà tựđộng tránh chướng ngại vật đường Theo lý thuyết, môi trường hoạt động robot tự hành địa hình Robot nghiên cứu robot di chuyển bánh (wheeled) Bài toán đặt điều khiển vị trí robot bám theo quỹ đạo đặt trước, ta thường chọn vector biến khớp q=[x y θ]T x,y tọa độ robot hệ tọa độ toàn cục, θ hướng robot Phụ thuộc vào hệ phương trình động lực học robot hệ có ràng buộc (nonholonomic) hay không ràng buộc (holonomic) ta chia robot tự hành thành robot có ràng buộc robot không ràng buộc Ví dụ robot không ràng buộc Omni Robot Các biến khớp robot lúc điều khiển tự do, robot di chuyển hoàn toàn linh hoạt Trong Mobile Robot thông thường, bánh xe đặt dọc theo trục robot Omni robot bánh xe đặt theo chu vi robot Nguyễn Quế Hà – Trần Dục Tú – ĐKTĐH –K56 Thiết kế điều khiển cho xe tự hành sử dụng thuật toán DSC I Cấu trúc xe tự hành (Omni Robot) Hình : Cấu trúc đơn giản xe tự hành Xe tự hành ? Robot tự hành định nghĩa loại xe robot có khả tự dịch chuyển , tự vận động ( lập trình ) điều khiển tự động để thực thành công công việc giao Các phận xe Bánh xe Omni Bánh xe Omni loại bánh có bánh vệ tinh nhỏ đặt xung quanh chu vi bánh, vòng bánh sole với vuông góc với trục bánh Nhờ bánh xe di chuyển dọc (do động truyền động) trượt theo chiều ngang (phụ thuộc vào chuyển động bánh lại) Kết hợp hai chuyển động (cộng vector ) bánh Omni di chuyển theo hướng giữ hướng đầu robot không đổi trình di chuyển ( Robot sử dụng bánh omni di chuyển theo hướng nào,ở góc mà không cần quay trước di chuyển ) Đó khác biết so với loại bánh xe truyền thống hay bánh xích dùng xe Tăng, đổi hướng phải quay đầu theo hướng Hình : Một số loại bánh xe omni Nguyễn Quế Hà – Trần Dục Tú – ĐKTĐH –K56 Thiết kế điều khiển cho xe tự hành sử dụng thuật toán DSC Vì có khả nên robot omni có khả tiết kiệm lượng so với robot sử dụng bánh xe thông thường Để thực khả di chuyển đó,robot omni phải có bậc tự mặt phẳng di chuyển (mp xoy),gồm: di chuyển độc lập theo trục x,y xoay theo trục z Do cần tối thiểu bánh omni với động điều khiển độc lập,vì động cho khả điều khiển bậc tự Hình : Khả di chuyển robot omni Bánh xe Omni sử dụng bánh tùy ý Ngoài bánh Omni cho tốc độ di chuyển hướng cho kết tốt di chuyển theo hướng chéo 45o Bánh xe Mecanum giống bánh xe omni bánh xe vệ tinh đặt chếch 45 độ Có loại bánh (trái, phải) khác hướng trục bánh xe vệ tinh Robot cần loại trái, phải để di chuyển linh hoạt Khi lắp lên đế robot bánh đặt thẳng bánh bình thường (cần ý trục bánh vệ tinh hướng tâm đế) Ưu điểm di chuyển động quay nhiều vòng bánh xe (giống nguyên lý xích xe đạp), trục động truyền gián tiếp qua dây sin buli nên chạy bánh xe không tác động lực trực tiếp vào động => Ít hại FET so với chuyển động đồng trục.Nhược điểm: Khi thiết kế khoảng cách buli trung gian trục động cơ, trục bánh xe bị chênh lệch, ko xác tuyệt đội bánh xe, chạy bánh xe ko giá thành đắt Hình :Hình ảnh bánh xe Mecanum Nguyễn Quế Hà – Trần Dục Tú – ĐKTĐH –K56 Thiết kế điều khiển cho xe tự hành sử dụng thuật toán DSC Cách di chuyển bánh xe omni Hình : Mô tả cách di chuyển omni robot Ta quy định động bánh xe (trong hình dùng kỹ thuật đồng trục, động bánh xe quay trục) quay theo chiều mũi tên chiều thuận kim đồng hồ + Đi thẳng trước: Bánh quay theo chiều mũi tên, bánh quay ngược chiều mũi tên + Đi thẳng sau: Bánh quay theo chiều mũi tên, bánh quay ngược chiều mũi tên + Sàng qua bên trái: Bánh quay theo chiều mũi tên, bánh quay ngược chiều mũi tên + Sàng qua bên phải: Bánh quay theo chiều mũi tên, bánh quay ngược chiều mũi tên + Sàng chéo tới theo trục động bánh số 1: Bánh ghì (dừng), bánh theo chiều mùi tên, bánh ngược chiều mũi tên + Sàng chéo tới theo trục động bánh số 2: Bánh ghì (dừng), bánh theo chiều mùi tên, bánh ngược chiều mũi tên + Sàng chéo tới theo trục động bánh số 3: Bánh ghì (dừng), bánh theo chiều mùi tên, bánh ngược chiều mũi tên + Sàng chéo tới theo trục động bánh số 4: Bánh ghì (dừng), bánh theo chiều mùi tên, bánh ngược chiều mũi tên + Xoay vòng tròn chỗ: qua phải (bánh 1, 2, 3, quay theo chiều mũi tên), qua trái (bánh 1, 2, 3, quay ngược chiều mũi tên) Khớp động (hình): giúp cho di chuyển gặp phải bề mặt gồ gề (mặt sân lồi lõm) bánh xe tiếp xúc với mặt sân Nguyễn Quế Hà – Trần Dục Tú – ĐKTĐH –K56 Thiết kế điều khiển cho xe tự hành sử dụng thuật toán DSC Sơ đồ vector biểu diễn chiều di chuyển robot sử dụng bánh xe omni Khi chuyển động tiến (forward) : Vector hai bánh phía trước hay phía sau biểu diễn sau: Hình : Sơ đồ vector chuyển động tiến Khi chuyển động lùi ( back ) : Vector hai bánh phía trước hay phía sau biểu diễn sau: Hình : Sơ đồ vector chuyển động lùi Khi chuyển động sang trái (left) : Vector hai bánh phía trước hay phía sau biễu diển sau: Hình : Sơ đồ vector chuyển động sang trái Khi chuyển động sang phải ( right ):Vector hai bánh phía trước hay phía sau biễu diễn sau: Hình : Sơ đồ vector chuyển động sang phải Nguyễn Quế Hà – Trần Dục Tú – ĐKTĐH –K56 Thiết kế điều khiển cho xe tự hành sử dụng thuật toán DSC Khi chuyển động theo đường chéo xe hai bánh xe nằm đường chéo thân xe chuyển động chiều hay ngược chiều Như thiết kế xe tự hành sử dụng bánh xe Omni chuyển động hướng khác Hình 10 :Sơ đồ vector mô tả hướng di chuyển robot omni bốn bánh Đặc biệt bánh xe chuyển động hướng xe chuyển động tròn theo hướng Thân xe Thân xe giá đỡ kim loại thiết kế để tối ưu không gian cho mạch điện tử động gắn lên để điều khiển xe di chuyển Sau số chi tiết thành phần 2.4.1 Cơ cấu chấp hành Ở ta sử dụng động Planet - Tích hợp sẵn encoder - Điện áp hoạt động 24VDC - Có giảm tốc để tăng momen tác dụng lên bánh xe 2.4.2 Mạch điện tử Bao gồm mạch công suất , cảm biến, khối nguồn mạch điều khiển - Mạch công suất sử dụng mạch cầu MOSFET - Khối nguồn sử dụng acquy 24V - Cảm biến sử dụng cảm biến hướng encoder tích hợp sẵn động - Mạch điều khiển sử dụng chip Atmega 128L Hình 11 : Sơ đồ mạch điện tử điều khiển động xe tự hành Nguyễn Quế Hà – Trần Dục Tú – ĐKTĐH –K56 10 Thiết kế điều khiển cho xe tự hành sử dụng thuật toán DSC Hình mô tả giản đồ di chuyển bánh Bánh cung cấp vận tốc v1 kết tổng hợp đạo hàm xm ym hệ tọa độ robot vận tốc góc L.𝜙̇ Hình 28 : Giản đồ di chuyển bánh - Với ψ góc quay bánh xe bánh xe ta có công thức sau: 𝑉1 = −𝑠𝑖𝑛𝛿𝑥̇ 𝑚 + 𝑐𝑜𝑠𝛿𝑦̇𝑚 + 𝐿𝜓̇ 𝑉2 = −𝑐𝑜𝑠𝛿𝑥̇ 𝑚 − 𝑠𝑖𝑛𝛿𝑦̇𝑚 + 𝐿𝜓̇ 𝑉3 = 𝑠𝑖𝑛𝛿𝑥̇ 𝑚 − 𝑐𝑜𝑠𝛿𝑦̇𝑚 + 𝐿𝜓̇ 𝑉4 = 𝑐𝑜𝑠𝛿𝑥̇ 𝑚 + 𝑠𝑖𝑛𝛿𝑦̇𝑚 + 𝐿𝜓̇ - Biểu diễn dạng ma trận ta có: 𝑉1 −𝑠𝑖𝑛𝛿 𝑉2 [ ] = [−𝑐𝑜𝑠𝛿 𝑉3 𝑠𝑖𝑛𝛿 𝑉4 𝑐𝑜𝑠𝛿 𝑐𝑜𝑠𝛿 −𝑠𝑖𝑛𝛿 −𝑐𝑜𝑠𝛿 𝑠𝑖𝑛𝛿 𝐿 𝑥̇ 𝑚 𝐿] [𝑦̇𝑚 ] 𝐿 ̇ 𝐿 𝜓 - Hay biểu diễn dạng: −𝑠𝑖𝑛𝛿 𝑥̇ 𝑚 𝑐𝑜𝑠𝛿 [𝑦̇𝑚 ] = 𝜓̇ [ 4𝐿 −𝑐𝑜𝑠𝛿 −𝑠𝑖𝑛𝛿 4𝐿 𝑠𝑖𝑛𝛿 𝑐𝑜𝑠𝛿 𝑉1 2 −𝑐𝑜𝑠𝛿 𝑠𝑖𝑛𝛿 𝑉2 [ ] 𝑉3 2 𝑉4 1 4𝐿 4𝐿 ] - Ta lại có : 𝑥̇ 𝑤 𝑐𝑜𝑠Ѳ −𝑠𝑖𝑛Ѳ [𝑦̇𝑤 ] = [ 𝑠𝑖𝑛Ѳ 𝑐𝑜𝑠Ѳ 0 Ѳ̇ Nguyễn Quế Hà – Trần Dục Tú – ĐKTĐH –K56 𝑥̇ 𝑚 0] [𝑦̇𝑚 ] 𝜓̇ (1.1) (1.2) 48 Thiết kế điều khiển cho xe tự hành sử dụng thuật toán DSC - Từ (1.1) (1.2) ta có: 𝑥̇ 𝑤 𝑐𝑜𝑠Ѳ [𝑦̇𝑤 ] = [ 𝑠𝑖𝑛Ѳ Ѳ̇ −𝑠𝑖𝑛Ѳ 𝑐𝑜𝑠Ѳ −𝑠𝑖𝑛𝛿 𝑐𝑜𝑠𝛿 0] 1 [ 4𝐿 −𝑐𝑜𝑠𝛿 −𝑠𝑖𝑛𝛿 4𝐿 𝑠𝑖𝑛𝛿 𝑐𝑜𝑠𝛿 𝑉1 2 −𝑐𝑜𝑠𝛿 𝑠𝑖𝑛𝛿 𝑉2 [ ] 𝑉3 2 𝑉4 1 4𝐿 4𝐿 ] −sin(Ѳ + 𝛿) −𝑐𝑜𝑠(Ѳ + 𝛿) 𝑠𝑖𝑛(Ѳ + 𝛿) 𝑐𝑜𝑠(Ѳ + 𝛿) 𝑉1 2 2 𝑐𝑜𝑠(Ѳ + 𝛿) −𝑠𝑖𝑛(Ѳ + 𝛿) −𝑐𝑜𝑠(Ѳ + 𝛿) 𝑠𝑖𝑛(Ѳ + 𝛿) 𝑉2 [ ] = 𝑉3 2 2 𝑉4 1 1 [ ] 4𝐿 4𝐿 4𝐿 4𝐿 1.2 Intelligent Motion Control for Four-Wheeled Holonomic Mobile Robots Using FPGA-Based Artificial Immune System Algorithm - Đối với báo mô hình động học là: −sin(Ѳ + 𝛿) 𝑥̇ 𝑐𝑜𝑠(Ѳ + 𝛿) [ 𝑦̇ ] = Ѳ̇ [ 4𝐿 −𝑐𝑜𝑠(Ѳ + 𝛿) 𝑠𝑖𝑛(Ѳ + 𝛿) 𝑐𝑜𝑠(Ѳ + 𝛿) 𝑉1 2 −𝑠𝑖𝑛(Ѳ + 𝛿) −𝑐𝑜𝑠(Ѳ + 𝛿) 𝑠𝑖𝑛(Ѳ + 𝛿) 𝑉2 [ ] 𝑉3 2 𝑉4 1 ] 4𝐿 4𝐿 4𝐿 𝑉1 𝑟𝜔1 𝑉2 𝑟𝜔2 - Với [ ] = [𝑟𝜔 ] ,r bán kính bánh xe 𝑉3 𝑟𝜔4 𝑉4 1.3 Trajectory Tracking Control Based On Adaptive Neural Dynamics For Four-Wheel Drive Omni-Directional Mobile Robots Hình 29 : Mô tả vận tốc bánh, góc lệch vận tốc góc robot Nguyễn Quế Hà – Trần Dục Tú – ĐKTĐH –K56 49 Thiết kế điều khiển cho xe tự hành sử dụng thuật toán DSC Như hình ta nhận thấy hệ tọa độ xe (Oxy) biểu diễn hệ tọa độ gốc (OXY) ma trận sau: 𝑐𝑜𝑠Ѳ 𝑅𝑚 = [−𝑠𝑖𝑛Ѳ g −𝑠𝑖𝑛Ѳ 𝑐𝑜𝑠Ѳ 0 0] - Chiếu thành phần vecto vận tốc tiếp tuyến với bánh xe lên trục tọa độ x y Hình 30 : Chiếu thành phần vecto vận tốc tiếp tuyến với bánh xe lên trục tọa độ x y - Từ ta biểu thức sau: 𝑉1 = −𝑠𝑖𝑛𝛿1 𝑥̇ 𝑚 + 𝑐𝑜𝑠𝛿1 𝑦̇𝑚 + 𝐿𝜓̇ 𝑉2 = −𝑠𝑖𝑛𝛿2 𝑥̇ 𝑚 − 𝑐𝑜𝑠𝛿2 𝑦̇𝑚 + 𝐿𝜓̇ 𝑉3 = 𝑠𝑖𝑛𝛿3 𝑥̇ 𝑚 − 𝑐𝑜𝑠𝛿3 𝑦̇𝑚 + 𝐿𝜓̇ 𝑉4 = 𝑠𝑖𝑛𝛿4 𝑥̇ 𝑚 + 𝑐𝑜𝑠𝛿4 𝑦̇𝑚 + 𝐿𝜓̇ - Biểu diễn dạng ma trận ta có: −𝑠𝑖𝑛𝛿1 𝑉1 𝑉 −𝑠𝑖𝑛𝛿2 [ 2] = [ 𝑉3 𝑠𝑖𝑛𝛿3 𝑉4 𝑠𝑖𝑛𝛿4 𝑐𝑜𝑠𝛿1 −𝑐𝑜𝑠𝛿2 −𝑐𝑜𝑠𝛿3 𝑐𝑜𝑠𝛿4 Nguyễn Quế Hà – Trần Dục Tú – ĐKTĐH –K56 𝐿 𝑥̇ 𝑚 𝐿 𝑦̇ ] [ 𝑚] 𝐿 𝜓̇ 𝐿 50 Thiết kế điều khiển cho xe tự hành sử dụng thuật toán DSC - Ta lại có: 𝑥̇𝑔 𝑥̇ 𝑚 g [𝑦̇𝑔 ] = 𝑅𝑚 [𝑦̇𝑚 ] 𝜓̇ Ѳ̇ 1.4 Trajectory Planning Optimization with Dynamics Modeling of Four-Wheeled Omni-Directional Mobile Robots Hình 31 : Mô tả vận tốc dài góc lệch bánh xe so với khung tọa độ robot - Chiếu Vi (i=1,2,3,4) lên trục xm,ym hệ tọa độ Oxmym ta có: 𝑉1 𝑉2 𝑉3 𝑉4 = −𝑠𝑖𝑛𝜃1 𝑥̇ 𝑚 = −𝑠𝑖𝑛𝜃2 𝑥̇ 𝑚 = −𝑠𝑖𝑛𝜃3 𝑥̇ 𝑚 = −𝑠𝑖𝑛𝜃4 𝑥̇ 𝑚 + 𝑐𝑜𝑠𝜃1 𝑦̇𝑚 + 𝑐𝑜𝑠𝜃2 𝑦̇𝑚 + 𝑐𝑜𝑠𝜃3 𝑦̇𝑚 + 𝑐𝑜𝑠𝜃4 𝑦̇𝑚 + 𝑙𝜙̇ + 𝑙𝜙̇ + 𝑙𝜙̇ + 𝑙𝜙̇ - Biểu diễn dạng ma trận ta có: −𝑠𝑖𝑛𝜃1 𝑉1 𝑉 −𝑠𝑖𝑛𝜃2 [ 2] = [ 𝑉3 −𝑠𝑖𝑛𝜃3 𝑉4 −𝑠𝑖𝑛𝜃4 𝑐𝑜𝑠𝜃1 𝑐𝑜𝑠𝜃2 𝑐𝑜𝑠𝜃3 𝑐𝑜𝑠𝜃4 Nguyễn Quế Hà – Trần Dục Tú – ĐKTĐH –K56 𝑙 𝑥̇ 𝑥̇ 𝑚 𝑚 𝑙 𝑦̇ ] [ 𝑚 ] = 𝐵 [𝑦̇𝑚 ] 𝑙 𝜙̇ 𝜙̇ 𝑙 (1.3) 51 Thiết kế điều khiển cho xe tự hành sử dụng thuật toán DSC Ta lại có: Từ (1.3) (1.4) ta có: 𝑥̇𝑔 𝑐𝑜𝑠𝜙 [𝑦̇𝑔 ] = [ 𝑠𝑖𝑛𝜙 Ѳ̇ −𝑠𝑖𝑛𝜙 𝑐𝑜𝑠𝜙 𝑥̇ 𝑚 𝑥̇ 𝑚 g 𝑦̇ 0] [ 𝑚 ] = 𝑅𝑚 [𝑦̇𝑚 ] 𝜙̇ 𝜙̇ 𝑉1 𝑟𝜔1 𝑥̇𝑔 𝑉 𝑟𝜔 [𝑦̇𝑔 ] = g𝑅𝑚 𝐵−1 [ ] = g𝑅𝑚 𝐵−1 [𝑟𝜔2 ] 𝑉3 Ѳ̇ 𝑟𝜔 𝑉 (1.4) (1.5) Từ (1.5) ta có: 𝑟𝜔̇ 𝑟𝜔1 𝑥̈𝑔 𝑟𝜔̇ 𝑟𝜔 [𝑦̈𝑔 ] = g𝑅̇𝑚 𝐵−1 [𝑟𝜔2 ]+ g𝑅𝑚 𝐵−1 [ ] 𝑟𝜔̇ 𝜃̈ 𝑟𝜔4 𝑟𝜔̇ Mô hình động lực học Theo tài liệu nước cách xây dựng mô hình mô hình động lực học omni robot khác Dưới số mô hình động lực học xây dựng tài liệu nước (Trajectory Planning Optimization with Dynamics Modeling of Four-Wheeled Omni-Directional Mobile Robots) Lực kéo chiều vận tốc bánh so với hệ tọa độ gốc kí hiệu fi (với i =1,2,3,4).Định nghĩa vecto Ft =[𝑓1 𝑓2 𝑓3 𝑓4 ]T - Fm lực momen hệ tọa độ robot định nghĩa :𝐹𝑚 = [𝐹𝑚𝑥 𝐹𝑚𝑦 𝑇𝑧 ]T Hình 32 : Mô tả lực kéo chiều vận tốc góc lệch bánh xe so với khung tọa độ robot - Chiếu vecto fi lên hệ tọa độ robot ta có : 𝐹𝑚𝑥 = −𝑠𝑖𝑛𝜃1 𝑓1 − 𝑠𝑖𝑛𝜃2 𝑓2 − 𝑠𝑖𝑛𝜃3 𝑓3 − 𝑠𝑖𝑛𝜃4 𝑓4 Nguyễn Quế Hà – Trần Dục Tú – ĐKTĐH –K56 52 Thiết kế điều khiển cho xe tự hành sử dụng thuật toán DSC 𝐹𝑚𝑦 = 𝑐𝑜𝑠𝜃1 𝑓1 + 𝑐𝑜𝑠𝜃2 𝑓2 + 𝑐𝑜𝑠𝜃3 𝑓3 + 𝑐𝑜𝑠𝜃4 𝑓4 𝑇𝑧 = 𝑙𝑓1 + 𝑙𝑓2 + 𝑙𝑓3 + 𝑙𝑓4 - Biểu diễn dạng ma trận ta có: 𝐹𝑚𝑥 −𝑠𝑖𝑛𝜃1 𝐹 [ 𝑚𝑦 ] = [ 𝑐𝑜𝑠𝜃1 𝑇𝑧 𝑙 - Hay: −𝑠𝑖𝑛𝜃2 𝑐𝑜𝑠𝜃2 𝑙 −𝑠𝑖𝑛𝜃3 𝑐𝑜𝑠𝜃3 𝑙 𝑓 𝑓1 −𝑠𝑖𝑛𝜃4 𝑓 𝑓 𝑐𝑜𝑠𝜃4 ] [ ] = 𝐵𝑇 [ ] 𝑓3 𝑓3 𝑙 𝑓 𝑓 4 𝑓1 𝐹𝑚𝑥 𝑓2 𝑇 −1 [ ] = (𝐵 ) [𝐹𝑚𝑦 ] 𝑓3 𝑇𝑧 𝑓4 2.1 Dynamic Control of An Omnidirectional Mobile Platform Hình 33 : Momen lực tác động lên robot - Để xác định mô hình động học người ta xác định hai vecto hệ tọa độ gốc sau: 𝑆 = [𝑥𝑤 𝑦𝑤 𝜃]T w 𝐹 = [𝐹𝑥 𝐹𝑦 𝑀𝐺 ]T w - Ta lại có hai vecto biểu diễn hệ tọa độ robot bởi: 𝑠 = [𝑥𝑚 𝑦𝑚 𝜃]T m 𝑓 = [𝑓𝑥 𝑓𝑦 𝑀𝐺 ]T m Nguyễn Quế Hà – Trần Dục Tú – ĐKTĐH –K56 53 Thiết kế điều khiển cho xe tự hành sử dụng thuật toán DSC - Công thức liên hệ hai tọa độ là: 𝑐𝑜𝑠𝜙 −𝑠𝑖𝑛𝜙 𝑆 = [ 𝑠𝑖𝑛𝜙 𝑐𝑜𝑠𝜙 0] m𝑠 = w𝑅𝑚 m𝑠 0 𝑐𝑜𝑠𝜙 −𝑠𝑖𝑛𝜙 w 𝐹 = [ 𝑠𝑖𝑛𝜙 𝑐𝑜𝑠𝜙 0] m𝑓 = w𝑅𝑚 m𝑓 0 w - Ta có: 𝑆̇ = w𝑅𝑚 m𝑠̇ w =>w𝑆̈= w𝑅𝑚 m𝑠̈ +w𝑅̇𝑚 m𝑠̇ - hay 𝑐𝑜𝑠𝜃𝑥̈ 𝑚 − 𝑠𝑖𝑛𝜃𝑦̈𝑚 − 𝑠𝑖𝑛𝜃𝑥̇ 𝑚 𝜃̇ − 𝑐𝑜𝑠𝜃𝑦̇𝑚 𝜃̇ w ̈ 𝑆 = [𝑠𝑖𝑛𝜃𝑥̈ 𝑚 + 𝑐𝑜𝑠𝜃𝑦̈𝑚 + 𝑐𝑜𝑠𝜃𝑥̇ 𝑚 𝜃̇ − 𝑠𝑖𝑛𝜃𝑦̇𝑚 𝜃̇] 𝜃̈ 𝑐𝑜𝑠𝜃 −𝑠𝑖𝑛𝜃 𝑥̈ 𝑚 − 𝑦̇𝑚 𝜃̇ = [ 𝑠𝑖𝑛𝜃 𝑐𝑜𝑠𝜃 0] [𝑦̈𝑚 + 𝑥̇ 𝑚 𝜃̇] 0 𝜃̈ ̇ 𝑥̈ 𝑚 − 𝑦̇𝑚 𝜃 w = 𝑅𝑚 [𝑦̈𝑚 + 𝑥̇ 𝑚 𝜃̇] 𝜃̈ - Theo định luật II Newton ta có: 𝑥̈ 𝑚 − 𝑦̇𝑚 𝜃̇ w 𝐹 = 𝑀 w𝑆̈=M w𝑅𝑚 [𝑦̈𝑚 + 𝑥̇ 𝑚 𝜃̇] 𝜃̈ 𝑥̈ 𝑚 − 𝑦̇𝑚 𝜃̇ w Lại có: w𝐹 = 𝑅𝑚 m𝑓 => m𝑓 = 𝑀 [𝑦̈𝑚 + 𝑥̇ 𝑚 𝜃̇](1) 𝜃̈ - Mặt khác: 𝑇 𝑓𝑥 −1 𝑇1 m 𝑓 = [ 𝑓𝑦 ] = [1 −1 0] [ ] (2) 𝑇3 𝐿 𝐿 𝑀𝐺 𝐿 𝐿 𝑇 𝜔1 𝐿 𝑥̇ 𝑚 𝜔2 −1 𝐿 𝑦̇ ] [ ](3) - Từ phương trình vận tốc =>[𝜔 ] = [ 𝑟 −1 𝐿 𝑚 ̇ 𝜔4 𝐿 𝜃 - Mô hình động học bánh xe đưa công thức sau: 𝐼𝑤 𝜔̇ 𝑖 + 𝑐𝜔𝑖 = 𝑘𝑢𝑖 − 𝑓𝑖 − 𝑟𝑇𝑖 - Với i=1,2,3,4 Hay Nguyễn Quế Hà – Trần Dục Tú – ĐKTĐH –K56 54 Thiết kế điều khiển cho xe tự hành sử dụng thuật toán DSC 𝑇𝑖 = (𝑘𝑢𝑖 − 𝐼𝑤 𝜔̇ 𝑖 − 𝑐𝜔𝑖 − 𝑓𝑖 ) 𝑟 - Từ (1) , (2) , (3) ta có: 𝑘 2𝐼 2𝑐 (𝑓 − 𝑓4 ) (−𝑢2 + 𝑢4 ) − 2𝜔 𝑥̈ 𝑚 − 𝑥̇ 𝑚 + 𝑟 𝑟 𝑟 𝑟 𝑘 2𝐼𝜔 2𝑐 (𝑓3 − 𝑓1 ) 𝑀(𝑦̈𝑚 + 𝑥̇ 𝑚 𝜃̇) = (−𝑢3 + 𝑢1 ) − 𝑦̈𝑚 − 𝑦̇𝑚 + 𝑟 𝑟 𝑟 𝑟 𝑘𝐿 4𝐼𝜔 4𝑐 (𝑓1 + 𝑓3 + 𝑓2 + 𝑓4 ) (𝑢 +𝑢 + 𝑢3 +𝑢4 ) − 𝐿 𝜃̈ − 𝐿 Ѳ̇ − 𝐼𝑚 𝜃̈ = 𝑟 𝑟 𝑟 𝑟 𝑀(𝑥̈ 𝑚 − 𝑦̇𝑚 𝜃̇) = - Hay: (𝑓2 − 𝑓4 ) 𝑘 2𝐼𝜔 2𝑐 ) 𝑥̈ 𝑚 + 𝑥̇ 𝑚 − 𝑀𝑦̇𝑚 𝜃̇ − = (−𝑢2 + 𝑢4 ) 𝑟 𝑟 𝑟 𝑟 (𝑓3 − 𝑓1 ) 𝑘 2𝐼𝜔 2𝑐 (𝑀 + ) 𝑦̈𝑚 + 𝑦̇𝑚 + 𝑀𝑥̇ 𝑚 𝜃̇ − = (−𝑢3 + 𝑢1 ) 𝑟 𝑟 𝑟 𝑟 (𝑀 + 4𝐼𝜔 4𝑐 (𝑓1 + 𝑓3 + 𝑓2 + 𝑓4 ) 𝑘𝐿 (𝑢 +𝑢 + 𝑢3 +𝑢4 ) 𝐿 )𝜃̈ + 𝐿2 𝜃̇ + = 𝑟 𝑟 𝑟 𝑟 Từ phương trình ta biễu diễn sạng sau: (𝐼𝑚 + (𝑀 + 2𝐼𝜔 𝑟2 ) (𝑀 + 0 [ −𝑘 𝑘 𝑟 𝑘𝐿 [𝑟 𝑟 0 −𝑘 𝑘𝐿 𝑟 𝑘𝐿 𝑟 𝑟 2𝐼𝜔 𝑟2 ) (𝐼𝑚 + 𝑘 4𝐼𝜔 𝐿 )] 𝑟2 2𝑐 𝑥̈ 𝑚 [𝑦̈𝑚 ]+ 𝜃̈ 𝑟2 (𝑓 −𝑓 ) 𝑥̇ 𝑚 − 𝑀𝑦̇𝑚 𝜃̇ − 𝑟 (𝑓 −𝑓 ) 𝑦̇ + 𝑀𝑥̇ 𝑚 𝜃̇ − = 𝑚 2𝑐 𝑟 [ 4𝑐 𝐿 𝜃̇ 𝑟2 + 𝑟 (𝑓1 +𝑓3 +𝑓2 +𝑓4 ) 𝑟 ] 𝑢1 𝑢 [𝑢2 ] => 𝑃{ m𝑠̈ }+N{ m𝑠̇ }=R{U}(4) 𝑘𝐿 𝑢 𝑟] 𝑟 - Thay m𝑠̇ = ( w𝑅𝑚 )T w𝑆̇ m𝑠̈ = ( w𝑅𝑚 )T (w𝑆̈ − w𝑅̇𝑚 ( w𝑅𝑚 )T w𝑆̇) vào (4) ta có: 𝑆̈ + 𝑁 ′ { w𝑆̇}= w𝑅𝑚 𝑃−1 R{U} 2𝑐 ⁄𝑟 𝑀 2𝑐 ⁄𝑟 𝑀 - Với 𝑁 ′ { w𝑆̇}= [ ′ 𝑥̇ 𝑤 − ( ′ + 1) 𝑦̇𝑤 𝜃̇ 𝑦̇𝑤 + ( ′ + 1) 𝑥̇ 𝑤 𝜃̇ ′ w 𝑀 𝑀 𝑀 Nguyễn Quế Hà – Trần Dục Tú – ĐKTĐH –K56 𝑀 4𝑐𝐿2 4𝐿2 𝐼𝑤 +𝑟 𝐼𝑚 𝜃̇ ] 55 Thiết kế điều khiển cho xe tự hành sử dụng thuật toán DSC 2.2 Modeling and Assessing of Omni-directional Robots with Three and Four Wheels Hình 34 : Mô hình robot tự hành chiều dương vận tốc góc,momen lực tác động lên robot 𝑑𝑣(𝑡) 𝑀 = ∑ 𝐹𝑣 (𝑡 ) − 𝐹𝐵𝑣 (𝑡 ) − 𝐹𝐶𝑣 (𝑡 ) 𝑑𝑡 𝑑𝑣𝑛(𝑡) 𝑀 = ∑ 𝐹𝑣𝑛 (𝑡 ) − 𝐹𝐵𝑣𝑛 (𝑡 ) − 𝐹𝐶𝑣𝑛 (𝑡 ) 𝑑𝑡 𝑑𝜔(𝑡) 𝐽 = ∑ 𝑇(𝑡 ) − 𝑇𝐵𝜔 (𝑡 ) − 𝑇𝐶𝜔 (𝑡 ) 𝑑𝑡 - Với 𝐹𝐵𝑣 (𝑡 ) = 𝐵𝑣 𝑣 (𝑡 ), 𝐹𝐵𝑣𝑛 (𝑡 ) = 𝐵𝑣𝑛 𝑣𝑛(𝑡 ), 𝑇𝐵𝜔 (𝑡 ) = 𝐵𝜔 (𝑡 )𝜔(𝑡 ) - Và 𝐹𝐶𝑣 (𝑡 ) = 𝐶𝑣 𝑠𝑖𝑔𝑛(𝑣 (𝑡 )), 𝐹𝐶𝑣𝑛 (𝑡 ) = 𝐶𝑣𝑛 𝑠𝑖𝑔𝑛(𝑣𝑛(𝑡 )), 𝑇𝐶𝜔 (𝑡 ) = 𝐶𝜔 𝑠𝑖𝑔𝑛(𝜔(𝑡 )) ∑ 𝐹𝑣 ( 𝑡) = 𝑓3 (𝑡 ) − 𝑓1 (𝑡) ∑ 𝐹𝑣𝑛 ( 𝑡) = 𝑓0 (𝑡 ) − 𝑓2 (𝑡) ∑ 𝑇( 𝑡) = (𝑓3 (𝑡 ) + 𝑓1 (𝑡 ) + 𝑓0 (𝑡 ) + 𝑓2 (𝑡))𝑑 - Mô hình động lực thiết bị chấp hành: Ở thiết bị chấp hành động không chổi than.Nó có mô hình tương tự với động chiều biểu diễn phương trình sau: 𝑢𝑗 (𝑡) = 𝐿 𝑑𝑖𝑗 (𝑡 ) + 𝑅𝑖𝑗 (𝑡) + 𝐾𝑣 𝜔𝑚𝑗 (𝑡) 𝑑𝑡 𝑇𝑚𝑗 (𝑡) = 𝐾𝑡 𝑖𝑗 (𝑡) Nguyễn Quế Hà – Trần Dục Tú – ĐKTĐH –K56 56 Thiết kế điều khiển cho xe tự hành sử dụng thuật toán DSC - Với: + L cuộn cảm động + R điện trở động + 𝐾𝑣 hệ số điện từ động + 𝑢𝑗 điện áp động + 𝜔𝑚𝑗 vận tốc góc động + 𝑇𝑚𝑗 momen động So sánh mô hình động học báo nước với nước Ở báo nước Hình 35 : Mô tả robot Omni bốn bánh theo tài liệu nước - Các phương trình động học mà ta biết với robot omni tọa độ ( x, y, ) vx (t ) dx(t ) / dt , vy (t ) dy (t ) / dt (t ) d (t) / dt - Phương trình cho phép chuyển đổi từ tuyến tính vận tốc vx v y trục tĩnh để tính vận tốc v robot v x (t ) v (t ) X R vn(t ) X v y (t ) (t ) (t ) ; cos( (t)) sin( (t)) X R sin( (t)) cos( (t)) X 0 (3.1) - Mối quan hệ vận tốc bánh xe vận tốc robot: Nguyễn Quế Hà – Trần Dục Tú – ĐKTĐH –K56 57 Thiết kế điều khiển cho xe tự hành sử dụng thuật toán DSC v0 (t ) v (t ) 1 v2 (t ) 1 v3 (t ) d v (t ) d vn(t ) d (t ) d (3.2) Có thể có phương trình xác định tốc độ Robot dựa theo tốc độ bánh xe ma trận phương trình không vuông Điều hệ thống thừa cấu chấp hành Ta viết lại: v(t ) (1/ 2).(v3 (t) v1 (t)) (3.3) vn(t ) (1/ 2).(v0 (t ) v2 (t )) (3.4) (t ) (v0 (t ) v1 (t ) v2 (t ) v3 (t )) / (4.d ) (3.5) Ở Việt Nam Hình 36 : Mô tả vận tốc dài bốn bánh xe - vận tốc dài pháp tuyến robot - Bốn bánh xe đặt lệch 90o , khoảng cách từ bánh đến tâm xe D - Oxy hệ toàn cục , V vận tốc thẳng xe , Vn vận tốc theo pháp tuyến xe 𝜔 vận tốc góc xe - q= [𝑥 𝑦 𝜃]𝑇 vector tọa độ hướng tâm xe hệ tọa độ toàn cục - Chiếu vector V,Vn lên hệ trục tọa độ toàn cục Oxy ta có 𝑥̇ 𝑐𝑜𝑠𝜃 𝑦̇ [ ]= [ 𝑠𝑖𝑛𝜃 𝜃̇ −𝑠𝑖𝑛𝜃 𝑐𝑜𝑠𝜃 0 𝑉 0] [𝑉𝑛] 𝜔 (3.6) Mặt khác [𝑉 𝑉𝑛 𝜔]𝑇 tính theo vận tốc bánh sau : Nguyễn Quế Hà – Trần Dục Tú – ĐKTĐH –K56 58 Thiết kế điều khiển cho xe tự hành sử dụng thuật toán DSC 𝑉 [𝑉𝑛] = 𝜔 o Với k = cos45 = √2 𝑘 −𝑘 −𝑘 𝑘 𝑘 𝑘 −𝑘 −𝑘 2 4𝐷 4𝐷 [4𝐷 𝑉1 𝑘 𝑉2 −𝑘 Và [ ] = [ 𝑉3 −𝑘 𝑉4 𝑘 𝑘 𝑘 −𝑘 −𝑘 𝑉1 𝑉2 [ ] 𝑉3 ] 𝑉4 (3.7) 𝐷 𝑉 𝐷 ] [𝑉𝑛] 𝐷 𝜔 𝐷 (3.8) 4𝐷 Thiết kế điều khiển - Với mô hình động học 𝑥̇ (𝑡) v(t) = P(θ(t)).[𝑦̇ (𝑡)] (4.1) ̇𝜃(𝑡) ta thiết kế điều khiển động học PI để đạt điểm ổn định theo dõi quỹ đạo cho robot omni di động có ràng buộc Ngoài , phương pháp điều khiển phi tuyến thống đồng thời giải ổn định vấn đề theo dõi quỹ đạo robot omni di động bánh Điểm ổn định Mục tiêu điều khiển điểm ổn định tìm vector vận tốc góc ( [𝜔1 𝑡 ), 𝜔2 (𝑡 ), 𝜔3 (𝑡 ), 𝜔4 (𝑡 )]𝑇 để định hướng robot di động có ràng buộc từ điểm bắt đầu [𝑥𝑜 𝑦𝑜 𝜃𝑜 ]𝑇 đến điểm đến mong muốn Zd= [𝑥𝑑 𝑦𝑑 𝜃𝑑 ]𝑇 Chú ý vị trí robot di động 𝑍1 (𝑡 ) = [𝑥(𝑡 ) 𝑦(𝑡 ) 𝑧(𝑡) ]𝑇 Để thiết kế điều khiển chuyển động việc xác định sai lệch vị trí vị trí điểm đến mong muốn 𝑥𝑑 𝑥𝑒 (𝑡) 𝑥(𝑡) 𝑍𝑒 (𝑡 ) = [𝑦𝑒 (𝑡)] = [𝑦(𝑡)] - [𝑦𝑑 ] (4.2) 𝜃 𝜃𝑒 (𝑡) 𝜃(𝑡) 𝑑 Để kiểm soát hệ thống ổn định tiệm cận ta đề xuất luật ổn định theo Với ma trận Kp KI đối xứng xác định dương Kp = diag{Kp1 , Kp2 , Kp3 } = KpT >0 , KI = diag{Ki1,Ki2,Ki3} = KIT > 0; 𝑡 𝜔1 (𝑡) ∫0 𝑥𝑒 (𝜏)𝑑𝜏 𝑥𝑒 (𝑡) 𝜔 (𝑡) 𝑡 [ ] = 𝑃(𝜃(𝑡 )) ∗ −𝐾𝑝 [𝑦𝑒 (𝑡)] − 𝐾𝐼 ∫0 𝑦𝑒 (𝜏)𝑑𝜏 (4.3) 𝜔3 (𝑡) 𝑟 𝑡 𝜃𝑒 (𝑡) 𝜔4 (𝑡) ∫0 𝜃𝑒 (𝜏)𝑑𝜏] [ ( ) Hệ thống điều khiển đề xuất phương trình tận dụng lợi lý thuyết điều khiển PID dùng rộng rãi hệ thống điều khiển công nghiệp Thay (3.3) vào (3.2) động lực học hệ thống báo lỗi vòng kín trở thành Nguyễn Quế Hà – Trần Dục Tú – ĐKTĐH –K56 59 Thiết kế điều khiển cho xe tự hành sử dụng thuật toán DSC 𝑡 ∫0 𝑥𝑒 (𝜏)𝑑𝜏 𝑥̇ 𝑒 (𝑡) 𝑥𝑒 (𝑡) 𝑡 [𝑦̇𝑒 (𝑡)] = −𝐾𝑝 [𝑦𝑒 (𝑡)] − 𝐾𝐼 ∫0 𝑦𝑒 (𝜏)𝑑𝜏 (4.4) ̇𝜃𝑒 (𝑡) 𝑡 𝜃𝑒 (𝑡) [∫0 𝜃𝑒 (𝜏)𝑑𝜏] - Vì ổn định tiệm cận hệ thống sai lệch vòng kín , hàm Lyapunov không bị chặn xuyên tâm chọn 𝑡 ∫0 𝑥𝑒 (𝜏)𝑑𝜏 𝑥𝑒 (𝑡) 𝑡 𝑡 𝑡 1 𝑡 𝑉1 (𝑡)=2 [𝑥𝑒 (𝑡)𝑦𝑒 (𝑡) 𝜃𝑒 (𝑡)] [𝑦𝑒 (𝑡)]+ [∫0 𝑥𝑒 (𝜏)𝑑𝜏 ∫0 𝑦𝑒 (𝜏)𝑑𝜏 ∫0 𝜃𝑒 (𝜏)𝑑𝜏].KI ∫0 𝑦𝑒 (𝜏)𝑑𝜏 𝜃𝑒 (𝑡) 𝑡 [∫0 𝜃𝑒 (𝜏)𝑑𝜏] (4.5) - Đạo hàm V1(t) , ta có 𝑥̇ 𝑒 (𝑡) 𝑥𝑒 (𝑡) ̇𝑉1 (𝑡) = [𝑥𝑒 (𝑡)𝑦𝑒 (𝑡) 𝜃𝑒 (𝑡)] [𝑦̇𝑒 (𝑡)] + [∫𝑡 𝑥𝑒 (𝜏)𝑑𝜏 ∫𝑡 𝑦𝑒 (𝜏)𝑑𝜏 ∫𝑡 𝜃𝑒 (𝜏)𝑑𝜏].KI [𝑦𝑒 (𝑡)] 0 𝜃𝑒 (𝑡) 𝜃̇𝑒 (𝑡) 𝑥𝑒 (𝑡) = - [𝑥𝑒 (𝑡)𝑦𝑒 (𝑡) 𝜃𝑒 (𝑡)] Kp [𝑦𝑒 (𝑡) ] 𝜃𝑒 (𝑡) (4.6) Vì 𝑉̇ bán xác định âm , Bổ đề Barbalat bao hàm [𝑥𝑒 (𝑡 )𝑦𝑒 (𝑡 ) 𝜃𝑒 (𝑡)]𝑇 → [0 0]𝑇 𝑘ℎ𝑖 𝑡 → ∞ Ngoài , dễ dàng cho thấy, gốc , [0 0]𝑇 ổn định tiệm cận toàn cục , Nghĩa là, sai lệch vị trí hướng tiếp cận tiệm cận đến không thời gian tiến đến vô Theo dõi quỹ đạo Không giống hầu hết robot di động không ràng buộc thông thường , quỹ đạo omni robot tạo cách sử dụng mô hình động học chúng Đưa quỹ đạo khả vi [𝑥𝑑 (𝑡 )𝑦𝑑 (𝑡) 𝜃𝑑 (𝑡)]𝑇 𝜖 𝐶 , xác định vector theo dõi sai lệch sau : 𝑥𝑒 (𝑡 ) 𝑥(𝑡) 𝑥𝑑 (𝑡 ) [𝑦𝑒 (𝑡 )] = [𝑦(𝑡)] - [𝑦𝑑 (𝑡 )] (4.7) 𝜃(𝑡) 𝜃𝑒 (𝑡 ) 𝜃𝑑 (𝑡 ) - Do 𝑟𝜔1 (𝑡) 𝑥̇ 𝑒 (𝑡) 𝑥̇ (𝑡) 𝑥̇ 𝑑 (𝑡) 𝑥̇ 𝑑 (𝑡) 𝑟𝜔2 (𝑡) −1 [𝑦̇𝑒 (𝑡)] = [𝑦̇ (𝑡)] - [𝑦̇ 𝑑 (𝑡)] = 𝑃 (𝜃(𝑡 )) [ ] - [𝑦̇ 𝑑 (𝑡)] (4.8) 𝑟𝜔3 (𝑡) ̇ ̇ ̇ ̇ 𝜃𝑒 (𝑡) 𝜃(𝑡) 𝜃𝑑 (𝑡) 𝜃𝑑 (𝑡) 𝑟𝜔4 (𝑡) Tương tự, mục tiêu điều khiển để tìm vector vận tốc góc [𝜔1 (𝑡 ), 𝜔2 (𝑡 ), 𝜔3 (𝑡 ), 𝜔4 (𝑡 )]𝑇 ví dụ hệ thống sai lệch vòng kín ổn định tiệm cận toàn cục Và đó, đưa luật theo dõi quỹ đạo sau : 𝑡 𝜔1 (𝑡) 𝑥 (𝜏)𝑑𝜏 ∫ 𝑥̇ 𝑑 (𝑡) 𝑒 𝑥𝑒 (𝑡 ) 𝜔2 (𝑡) 𝑡 [ ] = 𝑃(𝜃(𝑡 )) −𝐾𝑝 [𝑦𝑒 (𝑡 )] − 𝐾𝐼 ∫0 𝑦𝑒 (𝜏)𝑑𝜏 + [𝑦̇ 𝑑 (𝑡)] (4.9) 𝜔3 (𝑡) 𝑟 𝑡 𝜃𝑒 (𝑡) 𝜃̇𝑑 (𝑡) 𝜔4 (𝑡) ∫0 𝜃𝑒 (𝜏)𝑑𝜏] [ ( ) - Với Kp , KI ma trận đối xứng xác định dương Dẫn đến hệ thống điều chỉnh sai lệch vòng kín : Nguyễn Quế Hà – Trần Dục Tú – ĐKTĐH –K56 60 Thiết kế điều khiển cho xe tự hành sử dụng thuật toán DSC 𝑡 ∫0 𝑥𝑒 (𝜏)𝑑𝜏 𝑥̇ 𝑒 (𝑡) 𝑥𝑒 (𝑡 ) 𝑡 [𝑦̇𝑒 (𝑡)] = −𝐾𝑝 [𝑦𝑒 (𝑡 )] − 𝐾𝐼 ∫0 𝑦𝑒 (𝜏)𝑑𝜏 (4.10) ̇𝜃𝑒 (𝑡) 𝑡 𝜃𝑒 (𝑡) [∫0 𝜃𝑒 (𝜏)𝑑𝜏] Tương tự điểm ổn định, hàm Lyapunov chọn bán xác định âm giống công thức (3.5) , từ ta dễ dàng xác định hệ thống sai lệch vòng kín cho điều khiển theo dõi quỹ đạo ổn định tiệm cân Đáng nói vấn đề điểm ổn định điều khiển theo dõi quỹ đạo đồng thời đạt luật điều khiển (3.9) Luật điều khiển thống (3.9) trở thành điểm ổn định điểm đến mong muốn quỹ đạo phụ thuộc thời gian vị trí điểm đến cố định Điều khiển chiếu dựa theo dõi quỹ đạo - Phương pháp tiếp cận chiếu đơn áp dụng rộng rãi tới điều khiển chuyển động robot di động dựa điều khiển thiết kế với hoạt động ổn định - Ở ta phải chọn hàm Lyapunov khác với ta chọn 𝑥𝑒 (𝑡) V1 = [𝑥𝑒 (𝑡 )𝑦𝑒 (𝑡 ) 𝜃𝑒 (𝑡)] [𝑦𝑒 (𝑡)] 𝜃𝑒 (𝑡) 𝑥̇ 𝑒 (𝑡) - Đạo hàm theo thời gian ta có : 𝑉1̇ = [𝑥𝑒 (𝑡 )𝑦𝑒 (𝑡 ) 𝜃𝑒 (𝑡)] [𝑦̇𝑒 (𝑡) ] 𝜃̇𝑒 (𝑡) - Theo lý thuyết ổn định Lyapunov , ta đưa luật điều khiển chiếu 𝑘1 𝑥𝑒 + 𝑥̇ 𝑑 𝑐𝑜𝑠𝜃 + 𝑦̇ 𝑑 𝑠𝑖𝑛𝜃 𝑢 U = [ 𝑣 ] = [𝑘2 𝑦𝑒 − 𝑥̇ 𝑑 𝑐𝑜𝑠𝜃 + 𝑦̇ 𝑑 𝑠𝑖𝑛𝜃 ] (4.11) 𝜔 𝑘3 𝜃𝑒 + 𝜃𝑑 Dead – Reckoning - Là cách tính toán vị trí cách sử dụng vị trí biết trước dựa tốc độ theo thời gian - Thông tin tốc độ từ encoders bánh mô hình động học mô tả : 𝑥̇ (𝑡) [𝑦̇ (𝑡)] = 𝑃−1 (𝜙(𝑡 )) 𝑣(𝑡) (4.12) 𝜃̇(𝑡) - Để tìm vị trí robot ta sử dụng công thức Runge – Kutta để tính toán gần tư khác robot theo công thức sau : 𝑥(𝑘) 𝑥(𝑘 − 1) 𝑃−1 (𝜃(𝑘−1))𝑣(𝑘−1)+𝑃−1 (𝜃(𝑘))𝑣(𝑘) [𝑦(𝑘)] = [𝑦(𝑘 − 1)] + T (4.13) 𝜃(𝑘) 𝜃(𝑘 − 1) - Với T chu kỳ trích mẫu vô nhỏ x(k),y(k),z(k) giá trị vận tốc tức thời lần lấy mẫu thứ k Nguyễn Quế Hà – Trần Dục Tú – ĐKTĐH –K56 61 Thiết kế điều khiển cho xe tự hành sử dụng thuật toán DSC TÀI LIỆU THAM KHẢO [1] : Lý thuyết điều khiển phi tuyến – Nguyễn Doãn Phước, Phan Xuân Minh, Hán Thành Trung – NXB Khoa học kỹ thuật [2] : Dynamic Surface Control of Uncertain Nonlinear Systems - Bongsob Song, J.Karl Hedrick – Springer [3] : Precise Modeling of a Four Wheeled Omni – directional Robot - H´elder P Oliveira, Armando J Sousa, A Paulo Moreira and Paulo J Cost [4] : Trajectory tracking control based on adaptive neural dynamics for four wheel drive omni- directional mobile robots – J.Wang , J.Chen, S.Ouyang, Y.Yang – Engineering review, Vol 34 , Issue 3, 235-243,2014 [5] : Dynamic control of an Omnidirectional Mobile Platform – Tai-Yu Wang, ChingChih Tsai, Der-An Wang – Journal of Nan Kai , Vol.7 , No.1, pp.9-18 (2010) [6] : Kinematics Control of an Omnidirectional Mobile Robot – Ching-Chih Tsai, LiBin Jiang , Tai-Yu Wang, Tung-Seng Wang – Proceedings of 2005 CACS Automatic Control Conference Tainan , Taiwan , Nov 18-19 , 2005 [7] : Trajectory Planning Optimization with Dynamic Modeling of four Wheeled Omni-Directional Mobile Robots – Ehsan hashemi, Maani Ghaffri Jadidi, Omid Bakhshaded Babarsad - CIRA,Korea,December 15-18,2009 [8] : Hybrid Navigation of a Four-Wheeled Tour – Guide Robot – Yuan-Pao Hsu, Ching-Chih Tsai, Zeng-Chung Wang, Yi-Jiang Feng, Hung-Hsing Lin – ICROS-SICE International Joint Conference 2009, August 18-21, 2009, Fukuoka International Congress Center,Japan [9] : Intelligent Motion Control for Four-Wheeled Holonomic Mobile – Hsu-Chih Huang – Hindawi Research Article, August , 2013 Nguyễn Quế Hà – Trần Dục Tú – ĐKTĐH –K56 62 [...]...Thiết kế bộ điều khiển cho xe tự hành sử dụng thuật toán DSC II Mô hình hóa xe tự hành Hình 12 : Sơ đồ khối mô hình điều khiển xe tự hành Nhiệm vụ của bài toán điều khiển ở đây là xác định được momen đặt vào các bánh xe omni theo thời gian để đưa ra tín hiệu điều khiển phù hợp cho bánh xe di chuyển đúng theo quỹ đạo mong muốn của người điều khiển Đối với omni robot thì quỹ đạo của nó sẽ... –K56 14 Thiết kế bộ điều khiển cho xe tự hành sử dụng thuật toán DSC - Chọn 𝑢 = 𝑥̇ 2𝑑 − 𝑓 − 𝐾2 𝑆2 2 Phương pháp DSC ( Dynamic Surface Control ) Lý thuyết chung Để khắc phục nhược điểm đã nêu của phương pháp MSS, người ta sử dụng thêm một bộ lọc thông thấp cho các đầu vào ảo.Theo như đã nêu ở phần trước cách làm này chính là thuật toán điều khiển DSC Ta sẽ đặt 𝑥̅2 = 𝑥̇ 1𝑑 − 𝑓 − 𝐾1 𝑆1 và 𝑥2𝑑 được tính qua... Backstepping Theo DSC 3.2.1 Quỹ đạo đặt là quỹ đạo thẳng [ x y θ ]T = [ 5t 3+5t atan(5/3) ]T Hình 20 : Sơ đồ Simulink khi quỹ đạo đặt là quỹ đạo thẳng theo DSC Hình 21 : Robot chạy bám theo quỹ đạo thẳng theo thuật toán DSC Nguyễn Quế Hà – Trần Dục Tú – ĐKTĐH 4 –K56 20 Thiết kế bộ điều khiển cho xe tự hành sử dụng thuật toán DSC Hình 22 : Sai lệch quỹ đạo khi quỹ đạo đặt là quỹ đạo tròn theo thuật toán DSC 3.2.2... 18 Thiết kế bộ điều khiển cho xe tự hành sử dụng thuật toán DSC 3.1.2 Quỹ đạo đặt là quỹ đạo tròn 𝜋 𝜋 𝜋 15 15 15 [x y θ] T = [10cos( 𝑡) 10sin( 𝑡) 𝜋 𝑡 + ]T 2 Hình 17 : Sơ đồ Simulink khi quỹ đạo đặt là quỹ đạo tròn theo Backstepping Hình 18 : Robot bám quỹ đạo tròn theo Backstepping Nguyễn Quế Hà – Trần Dục Tú – ĐKTĐH 4 –K56 19 Thiết kế bộ điều khiển cho xe tự hành sử dụng thuật toán DSC Hình 19 : Sai... 0 1 1 [0 0 𝑚𝑟 𝑘 0 0 0 𝑢 𝐽𝑟 𝐷] 3 Mô phỏng trên Matlab ( Sử dụng công cụ S-function ) Theo Backstepping 3.1.1 Quỹ đạo đặt là quỹ đạo thẳng [ x y θ ] T = [ 5t 3+5t atan(5/3) ] T Nguyễn Quế Hà – Trần Dục Tú – ĐKTĐH 4 –K56 17 Thiết kế bộ điều khiển cho xe tự hành sử dụng thuật toán DSC Hình 14 : Sơ đồ Simulink khi quỹ đạo đặt là quỹ đạo thẳng theo Backstepping Hình 15 : Robot chạy bám quỹ đạo thẳng theo... Thiết kế bộ điều khiển cho xe tự hành sử dụng thuật toán DSC 𝐹𝑥 - Ta cũng có B(q) M = [ 𝐹𝑦 ] 𝑀𝜃 Với Fx , Fy là tổng các lực theo phương Ox , Oy 𝑀𝜃 là tổng momen quay của robot - Gọi r là bán kính bánh xe ta sẽ có 𝜋 B(q) = 1 𝑟 3𝜋 3𝜋 𝜋 𝑐𝑜𝑠( 𝜃 + ) 𝑐𝑜𝑠(𝜃 + ) 𝑐𝑜𝑠(𝜃 − ) 𝑐𝑜𝑠( 𝜃 − ) 4 4 4 4 [ 𝑠𝑖𝑛(𝜃 + 𝜋) 𝑠𝑖𝑛( 𝜃 + 3𝜋) 𝑠𝑖𝑛( 𝜃 − 3𝜋) 𝑠𝑖𝑛(𝜃 − 𝜋) ] 4 4 4 4 𝑅 𝑅 𝑅 𝑅 III Thiết kế bộ điều khiển cho robot omni 4 bánh... Thiết kế bộ điều khiển cho xe tự hành sử dụng thuật toán DSC 𝑘 𝜏 𝑚𝑟 𝑘 𝑣𝑛̇ − 𝜔𝑣 = 𝜏 𝑚𝑟 𝑛 𝐷 𝜔̇ = 𝜏𝜔 𝐽𝑟 𝑣̇ − 𝜔𝑣𝑛 = 𝑘 𝑥 𝑣 - Ta xác đinh các biến trạng thái như sau: 𝑥1 = [𝑦] , 𝑥2 = [𝑣𝑛] và u= 𝜔 𝜃 - Từ đó ta có mô hình trạng thái của đối tượng Omni Robot như sau: 𝑐𝑜𝑠𝜃 −𝑠𝑖𝑛𝜃 𝑥̇ 1 = [ 𝑠𝑖𝑛𝜃 𝑐𝑜𝑠𝜃 0 0 𝜔𝑣𝑛 𝑥̇ 2 = 𝑢 + [−𝜔𝑣 ] 0 1 1 −1 −1 1 𝜏 1 1 1] [ 𝜏𝑛 ] = [−1 1 𝜏 4 1 𝜔 1 −1 𝜏 𝜏 𝑚𝑟 𝑛 𝐷 [ 𝐽𝑟 𝜏𝜔 ] 0 0] 𝑥2 1 Và: 𝜏1 1... đặt là quỹ đạo tròn Hình 23 : Sơ đồ Simulink khi quỹ đạo đặt là quỹ đạo tròn theo DSC Nguyễn Quế Hà – Trần Dục Tú – ĐKTĐH 4 –K56 21 Thiết kế bộ điều khiển cho xe tự hành sử dụng thuật toán DSC Hình 24 : Robot bám quỹ đạo tròn theo DSC Hình 25 : Sai lệch quỹ đạo khi quỹ đạo đặt là quỹ đạo tròn theo DSC 4 Code lập trình Sfunction Khối tạo quỹ đạo tròn function quy_dao_tron(block) %MSFUNTMPL_BASIC A Template... 𝑆12 + 𝑆22 + 𝜉 2 ≤ 2𝑝 với p>0 là tập kín và lồi.Như vậy 𝜉 luôn tìm được hằng số dương M để η ≤ M 1 - Như vậy : 𝑉̇ ≤ −2𝐾𝑉 + 2 - Theo định lý Lyapunov thì hệ ổn định −𝐾1 𝑆12 𝐾2 𝑆22 Áp dụng cho đối tượng Omni robot Tương tự với phương pháp MSS thì ta chỉ thay: Nguyễn Quế Hà – Trần Dục Tú – ĐKTĐH 4 –K56 15 Thiết kế bộ điều khiển cho xe tự hành sử dụng thuật toán DSC 𝑐𝑜𝑠𝜃 [ 𝑥̅2 = −𝑠𝑖𝑛𝜃 0 𝑠𝑖𝑛𝜃 𝑐𝑜𝑠𝜃 0 𝑐𝑜𝑠𝜃𝑥̇... sau, u được chọn là : 𝑢 = 𝑥̇ 2𝑑 − 𝐾2 𝑆2 - Từ (1.4) và (1.5) ta có hệ sau: - Chọn hàm Lyapunov 𝑉 = 𝑆1̇ = 𝑆2 − 𝐾1 𝑆1 𝑆2̇ = −𝐾2 𝑆2 𝑆12 +𝑆22 2 (1.5) (1.6) Từ (1.6) ta tính đạo hàm của V : Nguyễn Quế Hà – Trần Dục Tú – ĐKTĐH 4 –K56 13 Thiết kế bộ điều khiển cho xe tự hành sử dụng thuật toán DSC 𝑉̇ = 𝑆1 𝑆1̇ + 𝑆2 𝑆2̇ = −𝐾1 𝑆12 − 𝐾2 𝑆22 + 𝑆1 𝑆2 𝑆21 +𝑆22 - Sử dụng bất đẳng thức 𝑆1 𝑆2 ≤ ta có: 2 𝑉̇ ≤= 1 −𝐾1 𝑆12 ... theo trục robot Omni robot bánh xe đặt theo chu vi robot Nguyễn Quế Hà – Trần Dục Tú – ĐKTĐH –K56 Thiết kế điều khiển cho xe tự hành sử dụng thuật toán DSC I Cấu trúc xe tự hành (Omni Robot) Hình... loại bánh xe omni Nguyễn Quế Hà – Trần Dục Tú – ĐKTĐH –K56 Thiết kế điều khiển cho xe tự hành sử dụng thuật toán DSC Vì có khả nên robot omni có khả tiết kiệm lượng so với robot sử dụng bánh xe... tử điều khiển động xe tự hành Nguyễn Quế Hà – Trần Dục Tú – ĐKTĐH –K56 10 Thiết kế điều khiển cho xe tự hành sử dụng thuật toán DSC II Mô hình hóa xe tự hành Hình 12 : Sơ đồ khối mô hình điều khiển