Thiết kế thí nghiệm bộ điều khiển thích nghi Observer Backstepping cho rô bốt công nghiệp

Kỹ thuật điều khiển Tr. X. Kiên, B.H. Huế, Tr.Đ.Thuận, “Thiết kế thí nghiệm rô bốt công nghiệp.” 164 THIẾT KẾ THÍ NGHIỆM BỘ ĐIỀU KHIỂN THÍCH NGHI OBSERVER BACKSTEPPING CHO RÔ BỐT CÔNG NGHIỆP Trần Xuân Kiên1*, Bùi Hồng Huế2, Trần Đức Thuận3 Tóm tắt: Bài báo trình bày một phương pháp thiết kế thích nghi observer backstepping điều khiển cánh tay rô bốt có khớp nối mềm. Sử dụng bộ đo lường quán tính (IMU) trên cơ sở cảm biến MEMs và bộ lọc Kalman mở rộng, đo góc quay không liên kết cơ khí với trục của cánh tay rô bốt. Thực hiện điều khiển bán tự nhiên kiểm tra thuật toán điều khiển và khả năng sử dụng IMU ứng dụng công cụ mô phỏng thời gian thực của MATLAB và card kết nối PCI1711. Từ khóa: Thích nghi observer backstepping, Bộ đo lường quán tính, Bộ lọc Kalman mở rộng. 1. ĐẶT VẤN ĐỀ Cánh tay robot có thể coi là một tập hợp các khâu gắn liền với các khớp nếu bằng cách nào đó (dùng encoder chẳng hạn) để đo các góc quay tương đối giữa hai khâu có chung khớp thì tác sẽ xác định được ma trận cô sin chỉ phương của từng khớp. Tuy nhiên, sẽ có nhược điểm sau: - Việc lắp đặt quá nhiều các encoder sẽ tốn kém và gặp trở ngại trong quá trình lắp đặt, nối dây và bo điều khiển - Độ chính xác sẽ giảm do tích lũy sai số ở việc xác định góc quay của từng khâu, do vấn đề đàn hồi (sự biến dạng của trục quay và của các khâu), khe hở (độ rơ của các khớp). Thực tế không dễ dàng thiết kế thiết bị cơ khí hay điện tử để đo được tất cả các trạng thái của hệ thống, với mục đích thiết kế bộ điều khiển thỏa mãn các yêu cầu trên nhóm tác giả sử dụng phương pháp thiết kế bộ điều khiển thích nghi observer backstepping cho hệ thống rô bốt công nghiệp với cánh tay có khớp nối mềm, thuật toán trình bày trong phần 3 của bài báo, trình bày chi tiết về thiết kế thuật toán backstepping observer trong tài liệu [2]. Việc áp dụng IMU trong việc đo góc quay của các cánh tay rô bốt công nghiệp có rất nhiều ưu điểm do không cần liên kết với trục quay của rô bốt như sử dụng encoder tuyệt đối, giảm được giá thành mà vẫn đảm bảo được độ chính xác trong đo góc quay. Phần 2 của bài báo trình bầy về bộ đo lường quán tính (IMU) sử dụng cảm biến MEMs và thuật toán lọc KALMAN mở rộng, chi tiết về thuật toán được trình bày trong các tài liệu [3]. Phần 4 trong bài báo trình bày kết quả mô phỏng bán tự nhiên (online - thời gian thực) bộ điều khiển observer backstepping và cảm biến đo góc quay IMU điều khiển cánh tay rô bốt có khớp nối mềm. Thuật toán điều khiển được đưa vào model của Simulink, chạy thời gian thực nhờ công cụ Real Time Window Target, Real Nghiên cứu khoa học công nghệ Tạp chí Nghiên cứu KH&CN quân sự, Số Đặc san Viện Điện tử, 10 - 2015 165 Time Workshop, liên kết với thế giới vật lý bên ngoài nhờ card PCI1711 của Advantech. 2. THIẾT KẾ BỘ ĐO LƯỜNG QUÁN TÍNH DÙNG BỘ LỌC KALMAN VÀ CẢM BIẾN TỪ CÓ HIỆU CHỈNH 2.1. Hiệu chỉnh từ trường Do có nhiễu địa vật tác động nên cảm biến từ trường sẽ đưa ra thông tin sai lệch về góc. Tài liệu [3] đưa ra phương pháp hiệu chỉnh các cảm biến từ, các giá trị từ kế hiệu chỉnh , , hc hc hcx y zB B B theo các trục được biểu diễn như sau: 0 0 0 h c x x x x h c y y y y h c z z z z B S B B B S B B B S B B       (1) Trong đó Sx, Sy, Sz là hệ số tỉ lệ, B0x, B0y, B0z là độ lệch dọc theo ba trục của trường từ và , , x y zB B B là các hệ số đo các từ trường thành phần. Hệ số tỉ lệ và độ lệch có thể tìm được theo phương trình sau: max min max min max min max min max min max min max min max min max min max min max min max min AX 1, , AX 1, , AX 1, , y y z z x x x x x x x z z y y y y y y y x x z z z z z B B B B S M B B B B B B B B S M B B B B B B B B S M B B B B                            ; m ax min 0 m ax m ax min 0 m ax m ax min 0 m ax . 2 . 2 . 2 x x x x x y y y y y z z z z z B B B B S B B B B S B B B B S                       (2) max max max, ,x y zB B B , min min min, ,x y zB B B là các giá trị lớn nhất, nhỏ nhất đo được của từ trường theo các trục x, y, z. Để hiệu chuẩn từ trường sử dụng phương trình (1), phải quay cảm biến từ 360 độ và xác định các giá trị đo như bảng 1. Chuyển từ , ,x y zB hình dạng méo sang , , hc x y zB có dạng hình cầu. Bảng 1 là kết quả các giá trị đo, và tính toán các hệ số tỉ lệ và độ lệch của cảm biến từ loại HMC5883L, được đưa vào khối bộ lọc Kalman trong mô dul thiết kế trên Simulink trình bầy tại phần 2.2 [3]. Bảng 1. Tham số hiệu chỉnh cho cảm biến HMC5883L. m axxB minxB maxyB minyB maxzB minzB -3130 (nT) 3070 (nT) -2750 (nT) 3240 (nT) -2960 (nT) 3940 (nT) xS 0xB yS 0 yB zS 0zB 1,119 -3,33 1,15 -27,6 1 -51 2.2. Thiết kế bộ lọc Kalman Thuật toán Kalman mở rộng [3], với các phương trình được thể hiện trong bảng 2. Kỹ thuật điều khiển Tr. X. Kiên, B.H. Huế, Tr.Đ.Thuận, “Thiết kế thí nghiệm rô bốt công nghiệp.” 166 Phương trình động học hệ thống 1 1 1( )k k k kx f x u      (3.1) (0, )k ku N Q (3.2) Các phương trình đo lường ( )k k k kz h x v    (3.3) (0, )k kv N R (3.4) Ma trận hiệp biến ˆ ˆ([ ][ ] ) T k k k k kP x x x x      (3.5) Tính toán tiền nghiệm trạng thái 1 1 ˆ ˆ( ) ( ( ))k k kx x      (3.6) Tuyến tính hóa hệ thống 1 1 1 ˆ ( )k k k x x f x            (3.7) Tiền nghiệm ước đoán đo lường ˆ ( )k k kz h x   (3.8) Tuyến tính hóa đo lường ˆ ( )k k k x x h H x        (3.9) Tính toán ma trận hiệp biến 1 1 1 1( ) ( ) T k k k k kP P Q         (3.10) Tính hệ số Kalman ( ) ( )T Tk k k k k k kK P H H P H R    (3.11) Cập nhật trạng thái ˆ ˆ ˆ( ) ( ) ( )k k k k kx x K z z       (3.12) Cập nhật ma trận hiệp biến ( ) (1 ) ( )k k k kP K H P    (3.13) Hàm truyền trạng thái Tốc độ góc quay và độ trôi được giả sử là các quá trình ngẫu nhiên, tức là 1 , 1 1 , 1 k k k k k b kb b                   (4) Như trong tài liệu đã công bố [3] của nhóm tác giả xác định các trạng thái 1kx  , hàm truyền f , các trạng thái đo lường, ma trận h, H và ma trận hiệp biến của nhiễu hệ thống Q và nhiễu đo lường R. Nhóm tác giả thiết kế xây dựng bộ đo lường quán tính sử dụng các cảm biến MEMs (gia tốc kế và con quay tốc độ góc), bộ lọc Kalman phi tuyến gồm 10 trạng thái  0 1 2 3 1 2 3 1 2 3 T x q q q q b b b   , bi là độ trôi của con quay cảm biến vi cơ. Thông tin đo lường được lấy từ các cảm biến vi cơ (gia tốc kế và con quay) và các cảm biến từ trường được hiệu chỉnh như trên. 2.3. Thiết kế phần mềm Phát triển thuật toán Kalman trên chíp vi xử lý dsPIC dùng các công cụ thiết kế, biên dịch MATLAB/Simulink và Microchip (MP-LAB, C30 Compiler) thay thế cho lập trình dòng lệch (C/C++) thông thường. Các công cụ này cho phép thuận tiện, nhanh khi thiết kế, dễ dàng khi kiểm tra, gỡ rối. Quy trình thiết kế phần mềm, từ bước thiết kế trên modul Simulink, biên dịch và nạp xuống chíp. Môdul thực thi thuật toán Kalman mở rộng, khối vào (của các cảm biến MEMs và từ trường qua I2C), khối đưa tín hiệu ra (qua cổng COM2) của chíp, các khối khai báo, cấu hình cho chíp vi xử lý trong model trên Simulink thể hiện trên hình 4. Nghiên cứu khoa học công nghệ Tạp chí Nghiên cứu KH&CN quân sự, Số Đặc san Viện Điện tử, 10 - 2015 167 2.4. Thiết kế phần cứng Modul IMU gồm chíp vi xử lý dsPIC33f, các khối truyền thông và kết nối với các cảm biến vi cơ điện tử MEMS (dùng chuẩn I2C), giao tiếp truyền dữ liệu tới máy tính (qua cổng COM2). Các cảm biến MEMs là loại GY80 có các cảm biến gia tốc, con quay vi cơ và cảm biến từ. Thuật toán Kalman mở rộng với phép toán xử lý dấu phẩy động đã chạy được trên chíp trong thời gian thực [3]. Nhóm nghiên cứu đã làm chủ thuật toán và thực hiện hiệu chỉnh cảm biến từ trường (phương trình 1) giúp xác định chính xác được tư thế của vật thể trong không gian. u1 u2 u3 ymyMux u1 u2 u3 ymyMux u1 u2 u3 ymyMux C function call Update: gpsInit gpsParser Init UART 2 Config Baud: 115200 (-1.34%) Bytes / Step: 11.4 UART Configuration1 TX_Labview_MATLAB 0 1 2 3 4 5 TX Output Multiplexed for Matlab / Labview To PC C function call Update: getGpsMainData y Produce the GPS Main Data and update the AP State (lat lon hei cog sog) [gps.c] dsPIC MASTER 33fJ256MC710 40.01 MIPS Master Accel_sens PQR_sens Lat Mags SOG COG baro_alt Lon(deg) H(m)2 Euler_hat PQR_hat Position_hat Vned_hat a_bias g_bias unfiltered_euler alive_log valid_log qout KALMAN FILTER BLOCK accer X1 accer Y1 accer Z1 gyro X1 gyro Y1 gyro Z1 hmc X1 hmc Y1 hmc Z1 IN_ PUT FROM 9DOF Compile for dsPIC (double-click) Generate Code int16 double double double int16 int16 int16 double int16 int16 0 Constant1 Configure Model for dsPIC Target (double-click) Configure Model for dsPIC Hình 1. Môdul chương trình cho bộ đo lường quán tính IMU. 3. THIẾT KẾ BỘ ĐIỀU KHIỂN THÍCH NGHI OBSERVER BACKSTEPPING 3.1. Mô hình đối tượng Đối tượng điều khiển là tay máy rô bốt có khớp nối mềm được điều khiển nhờ một động cơ điện một chiều. Hình 2. Mô hình đối tượng rô bốt liên kết khớp nối mềm. Mô tả hệ thống. q1 d q2 N K N J2 mg J1 O q2 X Z Y Kỹ thuật điều khiển Tr. X. Kiên, B.H. Huế, Tr.Đ.Thuận, “Thiết kế thí nghiệm rô bốt công nghiệp.” 168 Khi đó khớp nối như là mô hình xoắn lò xo, phương trình động học của hệ thống là: 2 1 1 1 1 1 1 2 2 2 2 2 1 2 c o s 0 t b q J q F q K q m g d q N qK J q F q q K i N N L D i R i K q u                           (5) Trong đó: q1, q2 là vị trí góc của khớp nối và trục mô tơ, i là dòng cảm ứng, u là điện áp phần ứng, Quán tính J1, J2, các hằng số ma sát nhớt F1, F2, Hệ số lò xo K, hằng số mô men Kt, hệ số phản hồi suất điện động Kb, điện trở và cảm kháng phần ứng R, L, trọng lượng vật nặng m, vị trí điểm kết nối đến tâm trọng lực d, tỉ số truyền N và gia tốc trọng trường g là các tham số không xác định. 3.2. Thiết kế bộ điều khiển observer backstepping cho cánh tay rô bốt có khớp nối mềm Giả sử rằng chỉ có vị trí của khớp nối 1q được đo. Lựa chọn các biến trạng thái 1 1 2 1 3 2 4 2 5, , , ,q q q q i          . Phương trình động học trở thành. 1 2 1 2 2 2 1 1 1 1 3 4 2 2 4 1 5 4 2 2 2 5 5 4 c o s 1 t b Fm g d K y J J J N K FK J N N J J KR u L L L                                               (6) Rõ ràng hệ phương trình trên chưa là dạng phản hồi đầu ra [1]. Vi phân của đầu ra y hai lần chúng ta có được 2 Dy  ( dD dt là toán tử vi phân) và nhận được mô tả đầu vào – đầu ra [1]. 5 4 31 2 1 2 1 2 1 31 2 1 2 2 1 2 2 1 2 1 2 2 211 2 1 2 2 2 2 1 2 1 2 1 2 1 2 1 2 2 1 cos cos t t b t b K K F FR mgd D y u D y D y J J NL L J J J K KF F F FR K K D y L J J J L J J N J J K K FF F F K F K FR K K R mgd D y D y L J J N J J J J N J J J J L L J J                                                      1 2 2 2 2 2 1 2 1 2 1 2 1 1 2 cos ost b t b K K K KF K F K RFR K mgd R mgd Dy D y c y L J J N J J J J L N L L J L J J N                     Sử dụng phương trình này nhưng để tìm lựa chọn biến trạng thái theo dạng phản hồi đầu ra. 1 2 1 2 3 2 2 3 4 4 5 4 5 6 7 5 0 8 1 . . .cos . .cos . .cos . .cos x x y x x y y x x y y x x y y x b u y y x                            (7) Nghiên cứu khoa học công nghệ Tạp chí Nghiên cứu KH&CN quân sự, Số Đặc san Viện Điện tử, 10 - 2015 169 Ở đó: 1 2 3 8, , ,.....    là các tham số không xác định tính theo [2]. 1 2 1 0 3 1 2 1 2 1 1 2 1 2 2 2 1 2 2 1 2 1 2 11 2 1 2 2 5 52 2 1 2 1 2 1 2 1 2 1 2 2 1 6 ; ; ; ; t t b t b K KF FR mgd b L J J J J NL J K KF F F FR K K L J J J L J J N J J K K FF F F K F K FR K K R mgd L J J N J J J J N J J J J L L J J FR L                                                                1 2 27 82 2 2 1 2 1 2 1 2 1 1 2 ; ;t b t b K K K KK F K RFK mgd R mgd J J N J J J J L N L L J L J J N                       (8) Theo [2] với các luật thích nghi: (2) (2) (1)1 1 1sgn( ) ( , , , ). ;m rb y y z      2 2 2 3 3 3 4 4 4 5 5 5. ; . ; . ; .z z z z                   (9) Thực hiện các 5 bước tính toán obsever backstepping như trong tài liệu [2], chọn các hàm Lyapunov từ V1 đến V5, sau khi tính toán bộ điều khiển u được xác định:         2 4 4 5 5 4 5 5 0 ,1 02 0 ,1 5 5 4 4 0 0 0 0 00 4 4 4 0 1 1 0 1 . . . 5. ( ) . . ( ) . ( ) . .sgn( ) . T p i j j i m i m r j i r u c z z d z k y y y A k y y A y A v b z y v y                                                                (10) Và 5 2 5 1 3 . . 4 T i i i i V c z d             . Theo lý thuyết LaSelle-Yoshizawa đảm bảo giới hạn toàn cục của: 0,1 0,5 0, 8,1 8,5( ), ( ), ....., ( ), ...... ( )...... ( ), ....., ( )ix t t t t t t     và sai số bám sát  lim ( ) ( ) 0t ry t y t   . 3.3. Khảo sát đánh giá trên mô hình thí nghiệm điều khiển bán tự nhiên Luật điều khiển (10) và luật thích nghi (9) được xây dựng trên mô hình Simulink, chạy thời gian thực nhờ công cụ Real Time Windows Target, liên kết với thế giới vật lý thực nhờ card giao tiếp PCI1711, điều khiển cánh tay rô bốt. Hình 3. Sơ đồ cấu trúc của thiết bị thí nghiệm bán tự nhiên HiL. Kỹ thuật điều khiển Tr. X. Kiên, B.H. Huế, Tr.Đ.Thuận, “Thiết kế thí nghiệm rô bốt công nghiệp.” 170 Trên hình 3 là sơ đồ khối mô hình thí nghiệm, bao gồm phần mềm: MATLAB/ Simulink; phần cứng: Máy tính nhúng, Card chuyên dụng ADVANTECH PCI 1711 có khả năng liên kết với Matlab. Trên hình 4 là giá thí nghiệm, đối tượng điều khiển là rô bốt thông dụng của Nhật Puma, thay thế động cơ xoay chiều và bộ điều khiển bằng động cơ một chiều và bộ điều chế độ rộng xung PWM tự chế tạo, thay các encoder tuyệt đối đo góc quay tại các khớp bằng thiết bị đo góc (IMU), tại khớp quay theo mặt phẳng đứng lắp thêm lò xo (khớp nối mềm) để có được mô hình cánh tay rô bốt như hình 2. Hình 4. Hình ảnh mô hình thí nghiệm. 1- Động cơ 1 chiều và hộp giảm tốc 1, 2- Cảm biến đo tốc độ góc w2, 3- Bộ điều chế độ rộng xung (PWM) và bộ khuếch đại công suất, 4- Lò so đàn hồi liên kết giữa động cơ và rô bốt, 5- Hộp giảm tốc 2, 6- Thân rô bốt, 7- Bộ IMU đo góc quay. 0 5 10 15 20 25 30 35 40 45 50 -30 -20 -10 0 10 20 30 25 -25 Thoi gian (giay) G oc (d o) 0 5 10 15 20 25 30 35 40 45 50 -50 -40 -30 -20 -10 0 10 20 30 40 50 Thoi gian (giay) To c d o ( vo ng /ph ut) Hình 5. Tín hiệu góc đặt, góc thực tế q1 và tín hiệu vận tốc góc w2. Kết quả thí nghiệm: Kết quả thử nghiệm (hình 5) minh chứng rằng thuật toán (10), luật thích nghi (hệ 9) thiết kế nhờ phương pháp observer backstepping đảm bảo hệ bám ổn định, thích nghi khi các tham số thay đổi (hệ có khớp nối mềm, khi 2 1 3 6 7 4 2 1 5 Nghiên cứu khoa học công nghệ Tạp chí Nghiên cứu KH&CN quân sự, Số Đặc san Viện Điện tử, 10 - 2015 171 thay đổi tốc độ góc về giá trị và hướng quay, các tham số điện từ, các tham số trong hệ (8)), kết quả trên hình 4 góc thực của trục cánh tay rô bốt đo được nhờ bộ đo lường IMU (đường 2) đảm bảo bám theo góc đặt (đường 1) sau thời gian 15 giây, sai số tĩnh, độ quá chỉnh, số dao động không tồn tại. 4. KẾT LUẬN Bài báo trình bày nghiên cứu áp dụng phương pháp thiết kế observer backstepping để thiết kế bộ điều khiển cho cánh tay rô bốt có khớp nối mềm. Bộ đo lường quán tính IMU dùng cảm biến vi cơ và thuật toán Kalman mở rộng để đo góc của trục cánh tay mà không cần liên kết cơ khí. Tiến trình thiết kế bộ điều khiển ứng dụng mô phỏng bán tự nhiên HiL để thử nghiệm thuật toán điều khiển trong phòng thí nghiệm. Kết quả minh chứng thuật toán điều khiển được thiết kế đảm bảo tính ổn định, tính thích nghi được khi tham số của hệ thay đổi. TÀI LIỆU THAM KHẢO [1] Krstic M., Kanellakopoulos L., Kokotovic P.V (1995), “Nonlinear And Adaptive Control Design”, A Wiley Interscience Publication John Wiley & Sons, Inc. [2] Huỳnh Văn Đông, Bùi Hồng Huế, Trần Xuân Kiên, Trần Đức Thuận, Nguyễn Mạnh Cường. “Tổng hợp Bộ điều khiển thích nghi Observer Backstepping cho Rô bốt có khớp nối mềm”. Tạp chí Nghiên cứu Khoa học Kỹ thuật và Công nghệ Quân sự. số tháng 9.2011 CH&ĐKTBB. p.170-178 [3] B. H. Hue, T. X. Kien, D. M. Dinh, D. D. Hanh, “Model-based Development and Implementation of Real-time Object Spatial Attitude Estimation”, International Journal of Computer and Electrical Engineering (IJCEE), Vol. 5, No. 4, pp. 372- 377, 2013. ABSTRACT DESIGN OF AN ADPTIVE OBSERVER BACKSTEPPING CONTROLLER FOR INDUSTRIAL ROBOTIC SYSTEMS The paper presents an adaptive observer backstepping control design approach for robot arms with spring joints. An Inertial Measurement Unit (IMU) with MEMS sensors and extended Kalman filtering is used to measure rotation angles without mechanical connection with robot arms. MATLAB real-time simulation tools and interface card PCI1711 are used for semi- natural (Hardware in the loop - HiL) control implementation to test the control algorithm and IMU capability. Keywords: Adaptive observer backstepping control, Inertial Measurement Unit (IMU), Extended Kalman filter. Nhận bài ngày 21 tháng 07 năm 2015 Hoàn thiện ngày 10 tháng 08 năm 2015 Chấp nhận đăng ngày 07 tháng 09 năm 2015 Địa chỉ: 1* Viện Điện tử, Viện Khoa học – Công nghệ quân sự; 2 Cao đẳng Xây dựng đô thị Hà nội; 3 Viện Khoa học – Công nghệ quân sự.