đề tài: “Nghiên cứu điều khiển rô bốt tay máy di động bám mục tiêu trên cơ sở sử dụng thông tin hình ảnh” để phát triển một số thuật toán điều khiển rô bốt theo dõi mục tiêu di động và rô bốt di chuyển bám mục tiêu sử dụng thông tin hình ảnh có nhiều tham số bất định. Đối tượng nghiên cứu Đối tượng nghiên cứu chính của đề tài là tập trung vào hệ rô bốt tay máy pantilt và rô bốt di động có cơ cấu di chuyển bằng bánh xe. Hệ camera dùng để lấy thông tin hình ảnh cho điều khiển tay máy bám mục tiêu di động là hệ 2 camera. Những hình ảnh thu được sẽ được xử lý, tính toán từ đó ra quyết định điều khiển các cơ cấu chấp hành khác của rô bốt thực hiện theo các yêu cầu. Mục tiêu nghiên cứu Mục tiêu của đề tài là nghiên cứu phát triển một số thuật toán điều khiển tay máy rô bốt bám mục tiêu di động dựa trên cơ sở thông tin hình ảnh sử dụng hai camera có tính bền vững với nhiều tham số bất định và khả năng kháng nhiễu với tốc độ xử lý cao. Đề xuất thuật toán điều khiển hệ rô bốt pantilt mang 2 camera bám mục tiêu di động với nhiều tham số bất định và xây dựng hệ 2 camera với ma trận Jacobian ảnh đầy đủ. Đề xuất một số thuật toán điều khiển hệ rô bốt di động, hệ pantilt mang 2 camera bám mục tiêu di động. Phạm vi nghiên cứu Nghiên cứu về hệ pantilt mang 2 camera (stereo camera) và rô bốt di động di chuyển bằng 3 bánh xe trong đó có 2 bánh chủ động, có nhiều tham số bất định bám mục tiêu di động. Nghiên cứu các thuật toán, phương pháp điều khiển hệ pantilt bám mục tiêu di động sử dụng thông tin hình ảnh từ hai camera. Nghiên cứu các thuật toán điều khiển hệ rô bốt di động, hệ pantilt stereo camera theo dõi và bám mục tiêu di động. Chỉ tập trung vào nghiên cứu sử dụng các kết quả mà camera thu được để điều khiển rô bốt, không đề cập tới phần hiệu chỉnh camera, thu nhận ảnh hay xử lý ảnh trên camera mà chỉ quan tâm tới việc xử lý từ điểm tính năng trên ảnh sau xử lý để đưa ra các tham số điều khiển rô bốt. Các yếu tố bất định, nhiễu trong các bài toán của luận án đều bị chặn. Phương pháp nghiên cứu Phương pháp nghiên cứu được áp dụng cho các vấn đề đã nêu ra của luận án như sau: 1. Trên cơ sở kế thừa các kết quả nghiên cứu đã có tác giả đi xây dựng mô hình toán học cho hệ thống rô bốt pantilt stereo camera và hệ thống phức hợp gồm rô bốt di động – hệ pantilt mang 2 camera khi có các tham số bất định trong mô hình và nhiễu. 2. Nghiên cứu các thuật toán điều khiển hiện đại để từ đó xây dựng các thuật toán điều khiển mới cho hệ pantilt theo dõi mục tiêu và hệ phức hợp nói trên bám mục tiêu di động. 3. Tối ưu hệ thống bao gồm tối ưu các ma trận Jacobian đảm bảo tính khả nghịch và tham số trong mô hình điều khiển. 4. Chứng minh tính ổn định của các thuật toán đề xuất bằng lý thuyết ổn định Lyapunov, bổ đề Barbalat. 5. Mô phỏng đáp ứng của hệ thống với các thuật toán đề xuất trên Matlab simulink để kiểm chứng việc theo dõi và bám mục tiêu của hệ thống. Ý nghĩa của đề tài Về mặt lý thuyết: đóng góp các thuật toán điều khiển hệ pantilt theo dõi mục tiêu và thuật toán điều khiển hệ rô bốt di động – rô bốt pantilt sử dụng 2 camera bám mục tiêu di động với nhiều tham số bất định trong mô hình cũng như nhiễu tác động. Về mặt thực tiễn: xây dựng mô hình camera 3D ảo loại bỏ sự suy biến của ma trận Jacobian ảnh và mô hình động lực học cho hệ gồm rô bốt di động – hệ pantilt mang dụng 2 camera. Những điểm mới của luận án Qua các nghiên cứu về phương pháp điều khiển rô bốt sử dụng thông tin hình ảnh, trong đó cụ thể là rô bốt pantilt và hệ rô bốt di động – rô bốt pantilt stereo camera có nhiều tham số bất định trong mô hình rô bốt, trong ma trận Jacobian ảnh, ma trận Jacobian rô bốt cũng như các yếu tố bên ngoài, tác giả có một số đóng góp như sau: Xây dựng 1 phương pháp để thiết lập ma trận Jacobian ảnh vuông cho hệ stereo camera giúp hệ rô bốt – stereo camera theo dõi được các đối tượng có chuyển động phức tạp dễ dàng hơn. Xây dựng bộ điều khiển mạng nơ ron nhân tạo để bù các tham số bất định trong mô hình rô bốt, trong ma trận Jacobian ảnh, ma trận Jacobian rô bốt cũng như các yếu tố bên ngoài. Bộ điều khiển này hoạt động tốt ngay cả khi tham số mô hình chỉ chắc chắn được 80% và ngay cả khi có các yếu tố bên ngoài tác động vào. Xây dựng mô hình động lực học cho hệ gồm rô bốt di động – hệ pantilt mang dụng 2 camera và 2 thuật toán điều khiển trong đó: 01 bộ điều khiển trượt và 01 bộ điều khiển tối ưu theo chuẩn tối ưu bình phương tối thiểu cho hệ kết hợp hai rô bốt gồm rô bốt pantilt stereo camera và rô bốt di động. Trong đó, mô hình động lực học của toàn hệ nói trên được tổng hợp trong một phương trình. Tối ưu hóa các tham số trong bộ điều khiển nơ ron. Mô phỏng kiểm chứng các thuật toán điều khiển rô bốt theo dõi và bám mục tiêu di động bằng công cụ mô phỏng Matlab. Công bố 07 công trình liên quan đến các nội dung nghiên cứu của luận án trên các tạp chí, kỷ yếu hội thảo trong nước và ngoài nước. Nội dung của luận án Luận án gồm 04 chương: Chương 1: Trình bày tổng quan về các vấn đề trong điều khiển rô bốt nói chung, điều khiển rô bốt sử dụng thông tin hình ảnh từ camera và đặc biệt là sử dụng hai camera để đưa ra định hướng nghiên cứu của luận án. Chương 2: Trình bày mô hình động học của hệ pantilt 2 camera. Xây dựng bộ điều khiển động học điều khiển hệ thống bám mục tiêu di động với nhiều tham số bất đinh trong mô hình toán học. Bộ điều khiển được xây dựng dựa trên phương pháp điều khiển PD kết hợp với mạng nơ ron RBF sử dụng thuật học online để bù các tham số bất định. Thuật toán được chứng minh tính ổn định tiệm cận bằng lý thuyết ổn định Lyapunov và mô phỏng kiểm chứng trên Matlab simulink
LỜI CAM ĐOAN Tôi xin cam đoan công trình nghiên cứu riêng tơi Các kết viết chung với tác giả khác đồng ý đồng tác giả trước đưa vào luận án Các kết luận án trung thực chưa công bố cơng trình khác Tác giả luận án Lê Văn Chung 11 LỜI CẢM ƠN Lời đầu tiên, Tôi xin chân thành cảm ơn Ban Lãnh đạo Học viện Khoa học Công nghệ, Viện Công nghệ thông tin - Viện Hàn lâm Khoa học Công nghệ Việt Nam, Phịng Cơng nghệ tự động hóa tạo điều kiện thuận lợi q trình học tập, nghiên cứu Tơi xin bày tỏ lịng kính trọng biết ơn sâu sắc tới PGS.TSKH Phạm Thượng Cát TS Phạm Minh Tuấn, hai thầy định hướng tận tình hướng dẫn để tơi hồn thành luận án Tơi xin cảm ơn Ban Giám hiệu Trường Đại học Công nghệ Thông tin Truyền thông - Đại học Thái Ngun, Khoa Cơng nghệ tự động hóa đơn vị Nhà trường quan tâm giúp đỡ, tạo điều kiện để tơi thực nghiên cứu Tơi xin cảm ơn cán Phịng Cơng nghệ tự động hóa – Viện Cơng nghệ thơng tin, đồng nghiệp thuộc Khoa Cơng nghệ Tự động hóa - Trường Đại học Công nghệ thông tin truyền thông - Đại học Thái Nguyên động viên trao đổi kinh nghiệm q trình hồn thành luận án Cuối cùng, xin chân thành cảm ơn đến gia đình, người thân, bạn đồng nghiệp - người ln dành cho tơi tình cảm nồng ấm, ln động viên sẻ chia lúc khó khăn sống tạo điều kiện tốt để hồn thành q trình nghiên cứu Hà Nội, ngày 18 tháng năm 2019 Tác giả luận án Lê Văn Chung 22 MỤC LỤC 33 DANH MỤC CÁC KÝ HIỆU VÀ CHỮ VIẾT TẮT Ký hiệu Đơn vị Mơ tả tính CTC Computed Torque Controller – điều khiển dựa d d(t) f fv f phương pháp tính mơ men Khoảng cách bánh xe rơ bốt di động Vecto ảnh hưởng nhiễu, tham số bất định Tiêu cự camera (1 pixel = 35μm) Tiêu cự thấu kính camera ảo Các thành phần bất định mơ hình động học hệ fu pan-tilt Các thành phần bất định mơ hình động lực m pixel pixel học hệ pan-tilt Hàm Hamilton Ma trận Jacobi ảnh Ma trận Jacobi rô bốt Ma trận Jacobi tổng hợp Các giá trị biết ma trận tương ứng Các đại lượng ma trận tương H Jimag Jrobot J Jˆ imag (m), Jˆ robot (q) ΔJ imag (m), ΔJ robot (q) ứng Ma trận giả nghịch đảo ma trận J; Jˆ = (Jˆ Jˆ ) Jˆ + T -1 T Jˆ + = ( Jˆ T Jˆ )- Jˆ T K k k1 l1 l2 LQR LQG m md m m m m m q qr Khoảng cách camera khoảng cách hai bánh xe rô bốt di động Khoảng cách rô bốt di động mục tiêu Chiều dài khớp pan rô bốt pan-tilt Chiều dài khớp tilt rô bốt pan-tilt Linear–Quadratic Regulator Linear–Quadratic–Gaussian Véc tơ đặc trưng ảnh Véc tơ đặc trưng ảnh mong muốn Tọa độ mục tiêu Véc tơ vị trí góc khớp pan, tilt Véc tơ vị trí góc khớp pan, tilt RBF s Tx Ty Tz Radial basis function neural network Véc tơ sai số giá trị mong muốn đo Vận tốc dài tay nắm camera Pixel Pixel Q m/s 44 ωx ωy ωz U, V m/s Pixel u1, u* v Vận tốc góc tay nắm camera Tọa độ ảnh đối tượng Thành phần điều khiển nơ ron Thành phần điều khiển tối ưu véc tơ biến đầu tay nắm bệ pan-tilt v Ω vs (trùng với gốc tọa độ camera OC) Véc tơ vận tốc dài đầu tay nắm bệ pan-tilt Véc tơ vận tốc góc đầu tay nắm bệ pan-tilt Véc tơ vận tốc đo bánh rô bốt di động vd khớp bệ pan-tilt Véc tơ vận tốc mong muốn đặt lên bánh rô bốt di x xs động khớp bệ pan-tilt Véc tơ mơ tả vị trí hướng camera Véc tơ tọa độ mục tiêu nhìn hệ tọa độ camera C C W ε θ1 θ2 θm θ1d θ2d θ md l rad rad rad rad rad rad m c j ,λ j φl , φr Stereo camera rad ảo Ma trận trọng số mạng nơ ron Sai số tham số mong muốn đo Góc quay khớp pan Góc quay khớp tilt Góc hướng rơ bốt di động Góc quay mong muốn khớp pan Góc quay mong muốn khớp tilt Góc hướng mong muốn rơ bốt di động Khoảng cách từ gốc tới vị trí đặt camera ảo Trọng tâm độ rộng khoảng cách tính từ tâm lớp ẩn thứ j radial basis function mạng nơ ron Góc quay bánh xe trái, phải rơ bốt di động Hệ camera gắn khung cố định rơ bốt pan-tilt có quan hệ hình học xác định trước 55 DANH MỤC HÌNH VẼ DANH MỤC BẢNG 66 MỞ ĐẦU Tính cấp thiết đề tài Rô bốt ứng dụng công nghiệp từ năm 60 để thay người làm công việc nặng nhọc, nguy hiểm môi trường độc hại Ngày rô bốt ứng dụng rộng rãi lĩnh vực sản xuất dịch vụ chế tạo máy, y tế, chăm sóc sức khỏe, nơng nghiệp, đóng tàu, xây dựng, an ninh quốc phịng gia đình … Nhu cầu sử dụng rô bốt ngành công nghiệp, dân dụng, dịch vụ an ninh quốc phòng gia tăng động lực cho phát triển rô bốt di động thông minh Cùng với phát triển mạnh mẽ hệ thống tự động hóa, rơ bốt di động 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 rơ bốt khả nhìn xử lý thơng tin hình ảnh để rơ bốt theo dõi đối tượng mục tiêu đứng yên di chuyển không gian, biết vị trí đứng mơi trường phi cấu trúc di chuyển tới vị trí khác, đồng thời tự động tránh chướng ngại vật đường Trong năm gần đây, nhiều cơng trình nghiên cứu điều khiển rơ bốt sử dụng thơng tin hình ảnh, kết đạt bộc lộ số hạn chế Chẳng hạn việc sử dụng camera rô bốt di động cho phép theo dõi đầy đủ thông tin mục tiêu biết trước mặt phẳng di chuyển mục tiêu hay việc sử dụng camera cho phép đáp ứng nhiều yêu cầu chưa xét tới suy biến ma trận Jacobian ảnh tác động tới khả bám hệ thống Bên cạnh mơ hình tốn học rơ bốt thường khó đạt độ xác tuyệt đối hệ thống chứa nhiều tham số bất định việc đo đạc tham số ban đầu hay hệ số ma sát, mơ men qn tính…, lại thường thay đổi q trình hoạt động Ngồi ra, tối ưu hóa tham số điều khiển rơ bốt để đạt độ xác mong muốn số trường hợp cụ thể vấn đề khó cần tiếp tục nghiên cứu Việc sử dụng thị giác camera xu hướng tất yếu mang tính linh hoạt dựa đặc tính hầu hết lồi động vật bậc cao có mắt Nhưng việc sử dụng thị giác camera cho rô bốt động, việc phát triển thuật toán điều khiển cho phát huy ưu điểm thị giác hai camera hạn chế khuyết điểm nêu ở nhiều vấn đề cần giải Với lý trên, tác giả lựa chọn đề tài: “Nghiên cứu điều khiển rô bốt tay máy di động bám mục tiêu sở sử dụng thơng tin hình ảnh” để phát triển số thuật tốn điều khiển rơ bốt theo dõi mục tiêu di động rô bốt di chuyển bám mục tiêu sử dụng thơng tin hình ảnh có nhiều tham số bất định Đối tượng nghiên cứu Đối tượng nghiên cứu đề tài tập trung vào hệ rô bốt tay máy pantilt rơ bốt di động có cấu di chuyển bánh xe Hệ camera dùng để lấy thơng tin hình ảnh cho điều khiển tay máy bám mục tiêu di động hệ camera Những hình ảnh thu xử lý, tính tốn từ định điều khiển cấu chấp hành khác rô bốt thực theo yêu cầu Mục tiêu nghiên cứu Mục tiêu đề tài nghiên cứu phát triển số thuật toán điều khiển tay máy rô bốt bám mục tiêu di động dựa sở thơng tin hình ảnh sử dụng hai camera có tính bền vững với nhiều tham số bất định khả kháng nhiễu với tốc độ xử lý cao - Đề xuất thuật toán điều khiển hệ rô bốt pan-tilt mang camera bám mục tiêu di động với nhiều tham số bất định xây dựng hệ camera với ma trận Jacobian ảnh đầy đủ - Đề xuất số thuật toán điều khiển hệ rô bốt di động, hệ pan-tilt mang camera bám mục tiêu di động Phạm vi nghiên cứu Nghiên cứu hệ pan-tilt mang camera (stereo camera) rô bốt di động di chuyển bánh xe có bánh chủ động, có nhiều tham số bất định bám mục tiêu di động Nghiên cứu thuật toán, phương pháp điều khiển hệ pan-tilt bám mục tiêu di động sử dụng thơng tin hình ảnh từ hai camera Nghiên cứu thuật toán điều khiển hệ rô bốt di động, hệ pan-tilt stereo camera theo dõi bám mục tiêu di động Chỉ tập trung vào nghiên cứu sử dụng kết mà camera thu để điều khiển rô bốt, không đề cập tới phần hiệu chỉnh camera, thu nhận ảnh hay xử lý ảnh camera mà quan tâm tới việc xử lý từ điểm tính ảnh sau xử lý để đưa tham số điều khiển rô bốt Các yếu tố bất định, nhiễu toán luận án bị chặn Phương pháp nghiên cứu Phương pháp nghiên cứu áp dụng cho vấn đề nêu luận án sau: Trên sở kế thừa kết nghiên cứu có tác giả xây dựng mơ hình tốn học cho hệ thống rơ bốt pan-tilt stereo camera hệ thống phức hợp gồm rô bốt di động – hệ pan-tilt mang camera có tham số bất định mơ hình nhiễu Nghiên cứu thuật toán điều khiển từ xây dựng thuật tốn điều khiển cho hệ pan-tilt theo dõi mục tiêu hệ phức hợp nói bám mục tiêu di động Tối ưu hệ thống bao gồm tối ưu ma trận Jacobian đảm bảo tính khả nghịch tham số mơ hình điều khiển Chứng minh tính ổn định thuật toán đề xuất lý thuyết ổn định Lyapunov, bổ đề Barbalat Mô đáp ứng hệ thống với thuật toán đề xuất Matlab simulink để kiểm chứng việc theo dõi bám mục tiêu hệ thống Ý nghĩa đề tài - Về mặt lý thuyết: đóng góp thuật toán điều khiển hệ pan-tilt theo dõi mục tiêu thuật tốn điều khiển hệ rơ bốt di động – rô bốt pantilt sử dụng camera bám mục tiêu di động với nhiều tham số bất định mô nhiễu tác động - Về mặt thực tiễn: xây dựng mơ hình camera 3D ảo loại bỏ suy biến ma trận Jacobian ảnh mô hình động lực học cho hệ gồm rơ bốt di động – hệ pan-tilt mang dụng camera Những điểm luận án Qua nghiên cứu phương pháp điều khiển rô bốt sử dụng thông tin hình ảnh, cụ thể rơ bốt pan-tilt hệ rô bốt di động – rô bốt pan-tilt stereo camera có nhiều tham số bất định mơ hình rơ bốt, ma trận Jacobian ảnh, ma trận Jacobian rơ bốt yếu tố bên ngồi, tác giả có số đóng góp sau: - Xây dựng phương pháp để thiết lập ma trận Jacobian ảnh vuông cho hệ stereo camera giúp hệ rô bốt – stereo camera theo dõi đối tượng có chuyển động phức tạp dễ dàng - Xây dựng điều khiển mạng nơ ron nhân tạo để bù tham số bất định mơ hình rơ bốt, ma trận Jacobian ảnh, ma trận Jacobian rô bốt yếu tố bên Bộ điều khiển hoạt động tốt tham số mơ hình chắn 80% có yếu tố bên ngồi tác động vào - Xây dựng mơ hình động lực học cho hệ gồm rô bốt di động – hệ pan-tilt mang dụng camera thuật tốn điều khiển đó: 01 điều khiển trượt 01 điều khiển tối ưu theo chuẩn tối ưu bình phương tối thiểu cho hệ kết hợp hai rô bốt gồm rô bốt pan-tilt stereo camera rơ bốt di động Trong đó, mơ hình động lực học tồn hệ nói tổng hợp phương trình Tối ưu hóa tham số điều khiển nơ ron Mô kiểm chứng thuật tốn điều khiển rơ bốt theo dõi bám mục tiêu di động công cụ mơ Matlab Cơng bố 07 cơng trình liên quan đến nội dung nghiên cứu luận án tạp chí, kỷ yếu hội thảo nước nước Nội dung luận án Luận án gồm 04 chương: Chương 1: Trình bày tổng quan vấn đề điều khiển rơ bốt nói chung, điều khiển rơ bốt sử dụng thơng tin hình ảnh từ camera đặc biệt sử dụng hai camera để đưa định hướng nghiên cứu luận án Chương 2: Trình bày mơ hình động học hệ pan-tilt camera Xây dựng điều khiển động học điều khiển hệ thống bám mục tiêu di động với nhiều tham số bất đinh mơ hình tốn học Bộ điều khiển xây dựng dựa phương pháp điều 10 %v_dot = tor; Mơ hình camera 3D ảo function {{X_S_dot,J] = fcn(lamda,fv, xyz_vd, xyz_vdot, theta1, theta2,l1,l2, theta1d, theta2d) %#codegen Rcv=[1, 0, 0; 0, 0, 1; 0, -1, 0]; xv = xyz_vd(1); yv = xyz_vd(2); zv = xyz_vd(3); j11 = 1/(lamda-xv); j12 = zv/(lamda-xv)^2; j21 = -(xv-lamda)/(zv+lamda)^2; j22 = 1/(zv+lamda); j31 = -(yv)/(zv+lamda)^2; 129 j33 = 1/(zv+lamda); Jvimg=[j11, j12, 0; j21, j22, 0; j31, , j33]; c1 = cos(theta1); c2 = cos(theta2); s1 = sin(theta1); s2 = sin(theta2); Rco = {{c1*c2, -c1*s2, s1; s1*c2, - s1*s2, -c1; s2, c2, 0]; Jrobot=[l2*c1*c2, 0; l2*s1*c2, 0; l2*s2, l1]; J=Jvimg*Rcv*Rco*Jrobot; thetad = {{theta1d;theta2d]; X_S_dot=fv*Jvimg*Rcv*Rco*xyz_vdot; Mơ hình stereo camera 130 function {{X_S, X_C]= fcn(J, q_dot, fv, t,lamda) %#codegen Rcv=[1, 0, 0; 0, 0, 1; 0, -1, 0]; X_S = J*q_dot; Xs = X_S(1); Ys = X_S(2); Zs = X_S(3); Xv =lamda*(Ys*fv+Xs+fv)/(Xs+fv^2); Zv = Xs*(lamda-Xv)/fv; Yv = Zs*(lamda+Zv)/fv; X_V = {{Xv;Yv;Zv]; X_C = Rcv\X_V; 131 Mơ hình mơ hệ phức hợp rô bốt di động mang pan-tilt stereo camera bám mục tiêu di động 132 Bộ điều khiển tối ưu Hàm tính u* function usao = fcn(e,e_dot) %#codegen R=[ 0.25 , 0, 0, 0; 0, 0.25, 0, 0; 0, 0, 0.25, 0; 0, 0, 0, 0.25]; alpha1=[15.5,0,0,0; 0,15.6,0,0; 0,0,15.6,0; 0,0,0,15.4]; e2=e_dot+alpha1*e; usao=-R\e2; 133 Bộ điều khiển động lực học hệ rô bốt di động – pan-tilt – stereo camera Vector a dot function a_vector_es_dot = fcn(v,vd,vd_dot,r,b,theta3,Learn) %#codegen c2 = cos(theta2); s2 = sin(theta2) c3 = cos(theta3); s3 = sin(theta3); v1d_dot = vd_dot(1); v2d_dot = vd_dot(2); v3d_dot = vd_dot(3); v4d_dot = vd_dot(4); v1d = vd(1); 134 v2d = vd(2); v3d = vd(3); v4d = vd(4); e = v - vd; theta3_dot = v(4); theta2_dot = v(3); theta1_dot = (r/b)*(v(1) - v(2)); y11 = (v1d_dot - v2d_dot)*(r/b)^2; y12 = (v1d_dot - v2d_dot)*(r/b)^2 + v3d_dot*(r/b); y13 = 0.5*(1 - cos(2*theta3))*(r/b)*((v1d_dot v2d_dot)*(r/b) + v3d_dot) + 0.5*(r/b)*sin(2*theta3)*theta3_dot*((v1d v2d)*(r/b) + v3d) + 0.5*(r/b)*sin(2*theta3)*(theta1_dot + theta2_dot)*v4d; y14 = 0.5*(1 + cos(2*theta3))*(r/b)*((v1d_dot v2d_dot)*(r/b) + v3d_dot) - 0.5*(r/b)*sin(2*theta3)*theta3_dot*((v1d v2d)*(r/b) + v3d) - 0.5*(r/b)*sin(2*theta3)*(theta1_dot + theta2_dot)*v4d; y15 = 0; y21 = -y11; y22 = -y12; y23 = -y13; y24 = -y14; y25 = 0; y31 = 0; 135 y32 = (v1d_dot - v2d_dot)*(r/b) + v3d_dot; y33 = 0.5*(1-cos(2*theta3))*((r/b)*(v1d_dot - v2d_dot) + v3d_dot) + 0.5*sin(2*theta3)*(theta3_dot*((r/b)*(v1d - v2d) + v3d) + (theta1_dot + theta2_dot)*v4d); y34 = 0.5*(1+cos(2*theta3))*((r/b)*(v1d_dot - v2d_dot) + v3d_dot) - 0.5*sin(2*theta3)*(theta3_dot*((r/b)*(v1d - v2d) + v3d) + (theta1_dot + theta2_dot)*v4d); y35 = 0; y41 = 0; y42 = 0; y43 = -0.5*sin(2*theta3)*(theta1_dot + theta2_dot)*((r/b)*(v1d - v2d) + v3d); y44 = 0.5*sin(2*theta3)*(theta1_dot + theta2_dot)*((r/b)*(v1d - v2d) + v3d); y45 = v4d_dot; Y = {{y11, y12, y13, y14, y15; y21, y22, y23, y24, y25; y31, y32, y33, y34, y35; y41, y42, y43, y44, y45]; a_vector_es_dot = -Learn*(Y')*e; Tor ************************************************************************ function Tor = fcn(vd_dot,vd,v,M1,M2,M3,r,b,Mw,Iw,Lf,L3,theta2,theta3,a_v ector_es,Kp) %#codegen 136 c2 = cos(theta2); s2 = sin(theta2); c3 = cos(theta3); s3 = sin(theta3); v1d_dot = vd_dot(1); v2d_dot = vd_dot(2); v3d_dot = vd_dot(3); v4d_dot = vd_dot(4); v1d = vd(1); v2d = vd(2); v3d = vd(3); v4d = vd(4); e = v - vd; theta3_dot = v(4); theta2_dot = v(3); theta1_dot = (r/b)*(v(1) - v(2)); y11 = (v1d_dot - v2d_dot)*(r/b)^2; y12 = (v1d_dot - v2d_dot)*(r/b)^2 + v3d_dot*(r/b); y13 = 0.5*(1 - cos(2*theta3))*(r/b)*((v1d_dot v2d_dot)*(r/b) + v3d_dot) + 0.5*(r/b)*sin(2*theta3)*theta3_dot*((v1d v2d)*(r/b) + v3d) + 0.5*(r/b)*sin(2*theta3)*(theta1_dot + theta2_dot)*v4d; y14 = 0.5*(1 + cos(2*theta3))*(r/b)*((v1d_dot 137 v2d_dot)*(r/b) + v3d_dot) - 0.5*(r/b)*sin(2*theta3)*theta3_dot*((v1d v2d)*(r/b) + v3d) - 0.5*(r/b)*sin(2*theta3)*(theta1_dot + theta2_dot)*v4d; y15 = 0; y21 = -y11; y22 = -y12; y23 = -y13; y24 = -y14; y25 = 0; y31 = 0; y32 = (v1d_dot - v2d_dot)*(r/b) + v3d_dot; y33 = 0.5*(1-cos(2*theta3))*((r/b)*(v1d_dot - v2d_dot) + v3d_dot) + 0.5*sin(2*theta3)*(theta3_dot*((r/b)*(v1d - v2d) + v3d) + (theta1_dot + theta2_dot)*v4d); y34 = 0.5*(1+cos(2*theta3))*((r/b)*(v1d_dot - v2d_dot) + v3d_dot) - 0.5*sin(2*theta3)*(theta3_dot*((r/b)*(v1d - v2d) + v3d) + (theta1_dot + theta2_dot)*v4d); y35 = 0; y41 = 0; y42 = 0; y43 = -0.5*sin(2*theta3)*(theta1_dot + theta2_dot)*((r/b)*(v1d - v2d) + v3d); y44 = 0.5*sin(2*theta3)*(theta1_dot + theta2_dot)*((r/b)*(v1d - v2d) + v3d); y45 = v4d_dot; 138 Y = {{y11, y12, y13, y14, y15; y21, y22, y23, y24, y25; y31, y32, y33, y34, y35; y41, y42, y43, y44, y45]; Z1 = 0.25*(M1 + M2 + M3)*(v1d_dot + v2d_dot)*r^2 + (Mw*r^2 + Iw)*v1d_dot + ((M2 + M3)*Lf^2 2*M3*Lf*L3*c2*s3)*(r/b)^2*(v1d_dot - v2d_dot) - M3*Lf*L3*c2*s3*(r/b)*v3d_dot + M3*Lf*L3*(s2*s3*theta2_dot c2*c3*theta3_dot)*(r/b)^2*(v1d - v2d) +M3*Lf*L3*(r/b)*((s2*s3*theta2_dot c2*c3*theta3_dot)*v3d + theta1_dot*(s2*s3*v3d c2*c3*v4d)); Z2 = 0.25*(M1 + M2 + M3)*(v1d_dot + v2d_dot)*r^2 + (Mw*r^2 + Iw)*v2d_dot - ((M2 + M3)*Lf^2 2*M3*Lf*L3*c2*s3)*(r/b)^2*(v1d_dot - v2d_dot) + M3*Lf*L3*c2*s3*(r/b)*v3d_dot M3*Lf*L3*(s2*s3*theta2_dot c2*c3*theta3_dot)*(r/b)^2*(v1d - v2d) - M3*Lf*L3*(r/b)*((s2*s3*theta2_dot c2*c3*theta3_dot)*v3d + theta1_dot*(s2*s3*v3d c2*c3*v4d)); Z3 = -M3*Lf*L3*c2*s3*(r/b)*(v1d_dot - v2d_dot) M3*Lf*L3*s2*s3*theta1_dot*(r/b)*(v1d - v2d) M3*Lf*L3*c2*c3*theta1_dot*v4d; %G = {{0; 0; 0; -9.8*M3*L3*sin(theta3)]; Z4 = M3*L3^2*v4d_dot* + 139 M3*Lf*L3*c2*c3*theta1_dot*(r/b)*(v1d - v2d) + M3*Lf*L3*c2*c3*theta1_dot*v3d -9.8*M3*L3*sin(theta3); Z = {{Z1; Z2; Z3; Z4]; Tor = Y*a_vector_es + Z - Kp*e; Động học hệ rô bốt di động – hệ pan-tilt – stereo camera function {{Zb_in_c, ImaF3, ImaF,X_0_es,Y_0_es,Z_0_es]= fcn(theta1,X,Y,theta2,theta3,muctieu,Lf,focus,px,py,pz,LZ, M) %#codegen c1 = cos(theta1); s1 = sin(theta1); c12 = cos(theta1 + theta2); 140 s12 = sin(theta1 + theta2); c3 = cos(theta3); s3 = sin(theta3); t11 = -s12; t12 = -c12*s3; t13 = c12*c3; t14 = c12*(px*c3 - py*s3) + s12*pz + X + Lf*c1; t21 = cos(theta1 + theta2); t22 = -sin(theta1 + theta2)*sin(theta3); t23 = sin(theta1 + theta2)*cos(theta3); t24 = s12*(px*c3-py*s3)- c12*pz + Y + Lf*s1; t31 =0; t32 = cos(theta3); t33 = sin(theta3); t34 = px*s3 + py*c3 + LZ; T = {{t11, t12, t13, t14; t21, t22, t23, t24; t31, 0, t32, t33, t34; 0, 0, 1]; Oc = T\muctieu; ImaF = -(focus/Oc(3))*[Oc(1); Oc(2)]; ImaF3 = -(focus/Oc(3))*[0.1+Oc(1); Oc(2);Oc(1)-0.1]; ul =ImaF3(1); vl =ImaF3(2); ur =ImaF3(3); 141 // // B =0.2; j11 = (ur-ul)/B; j13 = ul*(ul-ur)/(focus*B); j14 = ul*vl/focus; j15 = -(2*focus^2+ul^2+ul*ur)/(2*focus); j16 = vl; j22 = j11; j23 = vl*(ul-ur)/(focus*B); j24 = (focus^2+vl^2)/focus; j25 = -vl*(ur+ul)/(2*focus); j26 = -(ul+ur)/(2*focus); j31 = j11; j33 = ur*(ul-ur)/(focus*B); j34 = ur*vl/focus; j35 = -(2*focus^2+ur^2+ul*ur)/(2*focus); j36 = vl; Jimg = [j11, 0, j13, j14, j15, j16; 0, j22, j23, j24, j25, j26; j31, 0, j33, j34, j35, j36]; Jhe = [-s12, -c12, 0, 0, 0; -c12*s3,-s12*s3, 0, 0, 0; c12*c3, s12*c3, 0, 0, 0; , , 0, 0, -1; , , c3,s3,0; , , c3,s3,0]; J = Jimg*Jhe; q_dot = [xd; yd; t1d; t2d; t3d]; 142 M_dot = J*q_dot; B =0.2; ul_es = M(1); vl_es = M(2); ur_es = M(3); Zb_in_c = focus*B/(ur_es - ul_es); X_C = B*(ur_es + ul_es)/(2*(ur_es - ul_es)); Y_C = B*vl_es/(ur_es - ul_es); Ball_C_estimate = {{X_C; Y_C; Zb_in_c; 1]; Ball_0_estimate = T*Ball_C_estimate ; X_0_es = Ball_0_estimate(1); Y_0_es = Ball_0_estimate(2); Z_0_es = Ball_0_estimate(3); 143 ... mục tiêu sở sử dụng thông tin hình ảnh? ?? để phát triển số thuật tốn điều khiển rô bốt theo dõi mục tiêu di động rô bốt di chuyển bám mục tiêu sử dụng thông tin hình ảnh có nhiều tham số bất định... mục tiêu di động Nghiên cứu thuật toán, phương pháp điều khiển hệ pan-tilt bám mục tiêu di động sử dụng thơng tin hình ảnh từ hai camera Nghiên cứu thuật toán điều khiển hệ rô bốt di động, hệ... [59] sử dụng điều khiển trượt để điều khiển tay máy rô bốt sử dụng thông tin hình ảnh Yang Fang cộng [84] sử dụng điều khiển trượt 23 điều khiển rô bốt tự hành có sử dụng camera Tuy nhiên hệ động