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

Bạn đang xem bản rút gọn của tài liệu. Xem và tải ngay bản đầy đủ của tài liệu tại đây (352.15 KB, 7 trang )

(1)<div class='page_container' data-page=1>

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 qn 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
tố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 tốn được
trình bày trong các tài liệu [3].

</div>
(2)<div class='page_container' data-page=2>

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 Bhcx, Bhcy, Bzhc theo các trục được biểu diễn như 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

 

  (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à Bx, By, Bzlà 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, ,

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

S M

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

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 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 z

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

hc
x y z

B có dạng hình cầu. Bảng 1 là kết quả các giá trị đo, và

tính tố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 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 1 -51

2.2. Thiết kế bộ lọc Kalman

</div>
(3)<div class='page_container' data-page=3>

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 và độ trơi được giả sử là các quá trình ngẫu nhiên, tức là

1 , 1

k k k
k k b k

b 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 xk1,
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

</div>
(4)<div class='page_container' data-page=4>

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
y
myMux

u2
u3
y
myMux

C function call
Update: gpsInit
gpsParser Init

UART 2 Config
Baud: 115200 (-1.34%)

Bytes / Step: 11.4

UART Configuration1

TX_Labview_MATLAB
0

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 bộ đ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 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

m g

J1

O

q2

X Z

</div>
(5)<div class='page_container' data-page=5>

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

1 1 1 1 1 1

2 2 2 2 1

c o s 0

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 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 q1 được đo. Lựa chọn các biến trạng thái

1 q1, 2 q1, 3 q2, 4 q2, 5 i

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

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

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 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 (Dddtlà tốn tử vi phân) và nhận được
mơ tả đầu vào – đầu ra [1].

5 1 2 4 3

1 2 1 2 1

1 2 1 2

1 2 2 1 2 1 2

2 2

1 2 1 2 2

2 2

1 2 1 2 1 2 1 2 1 2 2 1

cos

t

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 2 2

1 2 1 2 1 2 1 1 2

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

. . .cos

. .cos . .cos

x x y x x y y

x x y y x x y y

</div>
(6)<div class='page_container' data-page=6>

Ở đó:  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

1 2 1 2 2

5 2 2 5

1 2 1 2 1 2 1 2 1 2 2 1

; ;

;
;

t

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

 



 



 

      

 

    

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

   

 

     

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

   

 

  1 2 2

7 8

2 2 2

1 2 1 2 1 2 1 1 2

; ;

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 các luật thích nghi: 1 sgn(bm)1( ,y (2),(2),yr(1)). ;z1

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

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

Thực hiện các 5 bước tính tố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 tốn bộ điều khiển u được xác

định:

















4 4

5 5 4 5 5 0 ,1 02 0 ,1 5 5

4 4

0 0 0 0

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

. .

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

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

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

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



y t( )y tr( )



0.

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.

</div>
(7)<div class='page_container' data-page=7>

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)

c (

)

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)

c do

ong

/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 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 khi các tham số thay đổi (hệ có khớp nối mềm, khi

2 
1

3

6

7 
4

</div>