Thiết kế thí nghiệm bộ điều khiển thích nghi observer backstepping cho rô bốt công nghiệ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ử [r]

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 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 đo lường quán tính (IMU) sở cảm biến MEMs lọc Kalman mở rộng, đo góc quay khơng liên kết khí với trục cánh tay rơ bốt Thực điều khiển bán tự nhiên kiểm tra thuật toán điều khiển khả sử dụng IMU ứng dụng công cụ mô thời gian thực MATLAB 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 coi tập hợp khâu gắn liền với khớp cách (dùng encoder chẳng hạn) để đo góc quay tương đối hai khâu có chung khớp tác xác định ma trận sin phương khớp Tuy nhiên, có nhược điểm sau:

- Việc lắp đặt nhiều encoder tốn gặp trở ngại trình lắp đặt, nối dây bo điều khiển

- Độ xác giảm tích lũy sai số việc xác định góc quay khâu, vấn đề đàn hồi (sự biến dạng trục quay khâu), khe hở (độ rơ khớp)

Thực tế không dễ dàng thiết kế thiết bị khí hay điện tử để đo tất trạng thái hệ thống, với mục đích thiết kế điều khiển thỏa mãn yêu cầu nhóm tác giả sử dụng phương pháp thiết kế đ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 tốn trình bày phần báo, trình bày chi tiết thiết kế thuật toán backstepping observer tài liệu [2]

Việc áp dụng IMU việc đo góc quay cánh tay rơ bốt cơng nghiệp có nhiều ưu điểm không cần liên kết với trục quay rô bốt sử dụng encoder tuyệt đối, giảm giá thành mà đảm bảo độ xác đo góc quay Phần báo trình bầy đo lường qn tính (IMU) sử dụng cảm biến MEMs thuật toán lọc KALMAN mở rộng, chi tiết thuật tốn trình bày tài liệu [3]

Time Workshop, liên kết với giới vật lý bên nhờ card PCI1711 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 đưa thơng tin sai lệch góc Tài liệu [3] đưa phương pháp hiệu chỉnh cảm biến từ, giá trị từ kế hiệu chỉnh Bhcx, Bhcy, Bzhc theo trục biểu diễn sau:

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 hệ số tỉ lệ, B0x, B0y, B0z độ lệch dọc theo ba trục

trường từ Bx, By, Bzlà hệ số đo từ trường thành phần Hệ số tỉ lệ độ

lệch tìm theo phương trình sau: max max

max max max max max max max max max max

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

0 m ax

m ax

0 m ax

m ax

0 m ax

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)

Bxm ax,Bymax,Bzm ax, Bxmin,Bymin,Bzmin giá trị lớn nhất, nhỏ đo từ trường theo 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 độ xác định giá trị đo bảng Chuyển từ

, ,

x y z

B hình dạng méo sang , ,

hc x y z

B có dạng hình cầu Bảng kết giá trị đo,

tính tốn hệ số tỉ lệ độ lệch cảm biến từ loại HMC5883L, đưa vào

khối lọc Kalman mô dul thiết kế Simulink trình bầy phần 2.2 [3]

Bảng 1. Tham số hiệu chỉnh cho cảm biến HMC5883L.

m ax x B

min

x

B Bymax Bymin Bzm ax Bzmin

-3130 (nT) 3070 (nT) -2750 (nT) 3240 (nT) -2960 (nT) 3940 (nT)

x

S B0x Sy B0y Sz B0z

1,119 -3,33 1,15 -27,6 -51

2.2 Thiết kế lọc Kalman

Phương trình động học hệ thống xk  fk1(xk1)uk1 (3.1) ukN(0,Qk) (3.2) Các phương trình đo lường zk hk(xk)vk (3.3)

(0, )

k k

v N R

 (3.4)

Ma trận hiệp biến

Pk ([xkxˆk][xk xˆk] )T (3.5) Tính tốn tiền nghiệm trạng thái xˆk( )  k1(xˆk1( )) (3.6)

Tuyến tính hóa hệ thống

1

1

ˆk ( )

k k

x x

f x



 

 



 

  

(3.7) Tiền nghiệm ước đoán đo lường zk h xkˆ ( )k  (3.8)

Tuyến tính hóa đo lường

ˆ ( )k

k k

x x

h H

x  

 

  

(3.9) Tính tốn ma trận hiệp biến Pk( )  k1Pk1( ) Tk1Qk1 (3.10)

Tính hệ số Kalman ( ) T ( ) T k k k k k k k

K P  H H P  H R (3.11)

Cập nhật trạng thái xˆk( ) xˆk( ) K zk(kzˆk) (3.12)

Cập nhật ma trận hiệp biến Pk( ) (1K Hk k)Pk( ) (3.13)

Hàm truyền trạng thái

Tốc độ góc quay độ trơi giả sử q trình ngẫu nhiên, tức

1 ,

1 ,

k k k k k b k

b b



  



 

 

 

 

  

   (4) Như tài liệu công bố [3] nhóm tác giả xác định trạng thái xk1, hàm truyền f , trạng thái đo lường, ma trận h, H ma trận hiệp biến nhiễu hệ thống Q nhiễu đo lường R Nhóm tác giả thiết kế xây dựng đo lường quán tính sử dụng cảm biến MEMs (gia tốc kế quay tốc độ góc), 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 độ trôi

con quay cảm biến vi Thông tin đo lường lấy từ cảm biến vi (gia tốc kế quay) cảm biến từ trường hiệu chỉnh

2.3.Thiết kế phần mềm

2.4.Thiết kế phần cứng

Modul IMU gồm chíp vi xử lý dsPIC33f, khối truyền thông kết nối với cảm biến vi điện tử MEMS (dùng chuẩn I2C), giao tiếp truyền liệu tới máy tính (qua cổng COM2) Các cảm biến MEMs loại GY80 có cảm biến gia tốc, quay vi 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 chíp thời gian thực [3] Nhóm nghiên cứu làm chủ thuật tốn thực hiệu chỉnh cảm biến từ trường (phương trình 1) giúp xác định xác tư vật thể không gian

u1 u2 u3 y myMux

u1 u2 u3 y myMux

u1 u2 u3 y myMux

C function call Update: gpsInit gpsParser Init

UART Config Baud: 115200 (-1.34%)

Bytes / Step: 11.4

UART Configuration1

TX_Labview_MATLAB

1

2

3

4

5

TX Output Multiplexed for Matlab / Labview

To PC C function call

Update: getGpsMainDatay

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 đo lường qn 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 tay máy rơ bốt có khớp nối mềm điều khiển nhờ động điện 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

m g

J1

O

q2

X Z

Khi khớp nối mơ hình xoắn lị xo, phương trình động học hệ thống là:

2

1 1 1

2

2 2

2

c o s

t b

q

J q F q K q m g d q

N q K

J q F q q K i

N N

L D i R i K q u

                        

  (5)

Trong đó: q1, q2 vị trí góc khớp nối trục mơ tơ, i dịng cảm ứng, u điện áp phần ứng, Quán tính J1, J2, số ma sát nhớt F1, F2, Hệ số lị xo K, số mơ men Kt, hệ số phản hồi suất điện động Kb, điện trở 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 gia tốc trọng trường g tham số không xác định

3.2 Thiết kế điều khiển observer backstepping cho cánh tay rơ bốt có khớp nối mềm

Giả sử có vị trí khớp nối q1 đo Lựa chọn biến trạng thái

1 q1, q1, q2, q2, i

            Phương trình động học trở thành

1

1

2

1 1

3

2

4

2 2

5

c o s

1

t

b

F

m g d K

y

J J J N

K F

K

J N N J J

K R

u

L L L

                                                (6)

Rõ ràng hệ phương trình chưa dạng phản hồi đầu [1] Vi phân đầu y hai lần có 2 Dy (Dddtlà tốn tử vi phân) nhận mơ tả đầu vào – đầu [1]

5

1 2

3

1 2

2

1 2 2

2

1

1 2

2

1 2 2 2

cos

cos

t

t b

t b

K K R F F mgd

D y u D y D y

J J NL L J J J

K K

F F F F

R K K

D y

L J J J L J J N J J

K K F

F F F K F K F

R 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

1 2 1

cos os

t b t b

K K K K

F K F K RF

R 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 để tìm lựa chọn biến trạng thái theo dạng phản hồi đầu

1 2 2

3 4 5

.cos

.cos cos

x x y x x y y

x x y y x x y y

Ở đó:  1, 2,3, 8 tham số khơng xác định tính theo [2]

1

1

1 2

1 2

2

1 2 2

1

1 2

5 2

1 2 2 2

6

; ;

; ;

t

t b

t b

K K F F

R mgd

b

L J J J J NL J

K K

F F F F

R K K

L J J J L J J N J J

K K F

F F F K F K F

R K K R mgd

L J J N J J J J N J J J J L L J J

F R L

 



 



 

      

 

    

       

   

 

     

            

   

 

  2

7

2 2

1 2 1

; ;

t b t b

K K K K

K F K K RF mgd R mgd

J J N J J J J L  N L L J  L J J N

     

      

     

 

 

 

(8)

Theo [2] với luật thích nghi: 1 sgn(bm)1( ,y (2),(2),yr(1)) ;z1

2 2.z2 ; 3.z3; 4.z4; 5.z5

            (9)

Thực bước tính tốn obsever backstepping tài liệu [2], chọn hàm Lyapunov từ V1 đến V5, sau tính tốn điều khiển u xác

định:

















2

4

5 5 ,1 02 ,1 5

4

0 0

0

4 4

0 1

0

( )

( ) ( )

.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à

2

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 ,5 , 8,1 8,5

( ), ( ), , ( ), i( ) ( ), , ( )

x t  t  t  t  t  t

sai số bám sát limt



y t( )y tr( )



0

3.3 Khảo sát đánh giá mơ hình thí nghiệm điều khiển bán tự nhiên

Luật điều khiển (10) luật thích nghi (9) xây dựng 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 giới vật lý thực nhờ card giao tiếp PCI1711, điều khiển cánh tay rô bốt

Trên hình 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 chun dụng ADVANTECH PCI 1711 có khả liên kết với Matlab Trên hình giá thí nghiệm, đối tượng điều khiển rơ bốt thông dụng Nhật Puma, thay động xoay chiều điều khiển động chiều điều chế độ rộng xung PWM tự chế tạo, thay encoder tuyệt đối đo góc quay khớp thiết bị đo góc (IMU), khớp quay theo mặt phẳng đứng lắp thêm lò xo (khớp nối mềm) để có mơ hình cánh tay rơ bốt hình

Hình 4 Hình ảnh mơ hình thí nghiệm

1- Động chiều 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) khuếch đại công suất, 4- Lò so đàn hồi liên kết động 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 10 15 20 25 30 35 40 45 50

-30 -20 -10 10 20 30 25

-25

Thoi gian (giay)

Go

c (

do

)

0 10 15 20 25 30 35 40 45 50

-50 -40 -30 -20 -10 10 20 30 40 50

Thoi gian (giay)

To

c

(v

ong

/ph

ut)

Hình 5. Tín hiệu góc đặt, góc thực tế q1 tín hiệu vận tốc góc w2

Kết thí nghiệm: Kết thử nghiệm (hình 5) minh chứng thuật tố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 tham số thay đổi (hệ có khớp nối mềm,

2 1

3

6

7 4