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
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
164
Tr. X. Kiên, B.H. Huế, Tr.Đ.Thuận, “Thiết kế thí nghiệm … rô bốt công nghiệp.”
Nghiên cứu khoa học công nghệ
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ừ
Bhc, Bhc, Bhc
kế hiệu chỉnh
theo các trục được biểu diễn như sau:
x
y
z
h c
B x S x B x B 0 x
h c
B y S B y B 0 y
y
h c
B z S z B z B 0 z
(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à là các hệ số đo các từ trường thành phần. Hệ số tỉ lệ và độ
Bx , By , Bz
lệch có thể tìm được theo phương trình sau:
Bymax Bymin
B
zmax B
Bx m ax Bx min
zmin
S MAX 1,
,
B0 x
B0 y
B0 z
Bx m ax .S
x
x
Bxmax Bxmin Bxmax Bxmin
2
(2)
By m ax By min
Bxmax Bxmin Bzmax Bzmin
Bymax Bymin Bymax Bymin
By m ax .S
S MAX 1,
,
y
y
2
Bz m ax Bz min
Bymax Bymin
Bxmax B
xmin
Bz m ax .S
S MAX 1,
,
z
z
2
Bzmax Bzmin Bzmax Bzmin
;
,
là các giá trị lớn nhất, nhỏ nhất đo được của từ
Bxmin , Bymin , Bzmin
Bxmax , Bymax , Bzmax
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ừ
hc
x, y,z
hình dạng méo sang
có dạng hình cầu. Bảng 1 là kết quả các giá trị đo, và
B
Bx,y,z
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.
Bxmax
By min
Bzmax
Bx min
Bymax
Bz min
-3130 (nT) 3070 (nT) -2750 (nT) 3240 (nT) -2960 (nT)
3940 (nT)
Sx
B0x
B0z
Sy
B0 y
Sz
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.
165
Tạp chí Nghiên cứu KH&CN quân sự, Số Đặc san Viện Điện tử, 10 - 2015
Kỹ thuật điều khiển
x f (x ) u
k k 1 k 1
Phương trình động học hệ thống
Các phương trình đo lường
k 1
(3.1)
u N(0,Q )
k
k
(3.2)
(3.3)
z h (x ) v
k
k
k
k
(3.4)
v N(0, R )
k
k
T
Ma trận hiệp biến
(3.5)
(3.6)
ˆ
ˆ
P ([x x ][x x ] )
k k k k
k
Tính toán tiền nghiệm trạng thái
Tuyến tính hóa hệ thống
ˆ
ˆ
x () (x ())
k k 1
k 1
fk 1
(3.7)
k 1
x
ˆ
x x ( )
k 1
Tiền nghiệm ước đoán đo lường
Tuyến tính hóa đo lường
(3.8)
(3.9)
ˆ
z h x ()
k
k k
hk
x
H k
ˆ
x x ()
k
P () k1P ()T k1 Qk1
Tính toán ma trận hiệp biến
Tính hệ số Kalman
(3.10)
(3.11)
(3.12)
(3.13)
k
k1
Kk P ()HkT Hk P ()HkT Rk
k
k
Cập nhật trạng thái
ˆ
ˆ
ˆ
x () x () K (z z )
k k
k
k
k
Cập nhật ma trận hiệp biến
P () (1 Kk H k )P ()
k
k
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à
k k1 ,k1
(4)
b b
k k1 b,k1
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 ,
k1
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
T
Kalman phi tuyến gồm 10 trạng thái
, bi là độ trôi của
x q q q q b b b
3
0
1
2
3
1
2
3
1
2
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.
166
Tr. X. Kiên, B.H. Huế, Tr.Đ.Thuận, “Thiết kế thí nghiệm … rô bốt công nghiệp.”
Nghiên cứu khoa học công nghệ
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.
C function call
Update: gpsInit
Configure Model for
dsPIC Target
(double-click)
dsPIC MASTER
33fJ256MC710
40.01 MIPS
UART 2 Config
Baud: 115200 (-1.34%)
Bytes / Step: 11.4
Compile for dsPIC
(double-click)
gpsParser Init
Configure Model
for dsPIC
Generate Code
Master
UART Configuration1
accer X1
accer Y1
accer Z1
gyro X1
gyro Y1
gyro Z1
hmc X1
hmc Y1
hmc Z1
Accel_sens
PQR_sens
Lat
Euler_hat
PQR_hat
Position_hat
Vned_hat
a_bias
double
yM
yu
u
y
y
M
u
u
double
double
Mags
yM
yu
u
SOG
g_bias
IN_ PUT FROM 9DOF
COG
unfiltered_euler
alive_log
valid_log
qout
0
baro_alt
Lon(deg)
H(m)2
Constant1
C function call
Update: getGpsMainData
y
double
Produce the GPS Main
Data and update the AP State
(lat lon hei cog sog)
[gps.c]
KALMAN FILTER BLOCK
int16
0
1
int16
int16
2
X_Labview_MATLA
3
int16
int16
4
int16
5
TX Output Multiplexed
for Matlab / Labview
To PC
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.
O
K
N
J1
J2
q2
N
mg
Z
X
q2
Y
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.
167
Tạp chí Nghiên cứu KH&CN quân sự, Số Đặc san Viện Điện tử, 10 - 2015
Kỹ thuật điều khiển
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à:
q 2
N
J 1 q1 F1 q1 K q1
m g d co s q1 0
K
N
q 2
N
(5)
J 2 q 2 F2 q 2
q1
K t i
L D i R i K b q 2 u
Trong đó: q , q là vị trí góc của khớp nối và trục mô tơ, i là dòng cảm ứng, u
1 2
là điện áp phần ứng, Quán tính J , J , các hằng số ma sát nhớt F , F , Hệ số lò xo
1
2
1
2
K, hằng số mô men K , hệ số phản hồi suất điện động K , điện trở và cảm kháng
t
b
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
được đo. Lựa chọn các biến trạng thái
q1
. Phương trình động học trở thành.
1 q1 ,2 q1,3 q2 ,4 q2 ,5 i
1
2
m g d
J 1
F1
J 1
K
N
2
c o s y
2
2
1
(6)
J 1
3
4
4
K
N
K
J
F 2
2
t
1
5
4
J
2 N
R
J
2
2
K
1
b
u
5
5
4
L
L
L
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
d
ra y hai lần chúng ta có được
(
là toán tử vi phân) và nhận được
2 Dy
D
dt
mô tả đầu vào – đầu ra [1].
Kt K
R
L
F
F2
mgd
J1
D5 y
u
D4 y
D3 cos y
1
J1 J2 NL
J1 J2
R
L
F
F2
Kt Kb
J2 L
K
K
F F2
D3 y
1
1
J1 J2
J1 J2 N 2 J1 J2
R
L
K
K
F F2
F K
F2 K Kt Kb F
R
L
F2 mgd
1
1
1
D2 y
D2 cos y
J1 J2 N 2 J1 J2
J1 J2 N 2 J1J2
J1J2 L
J2
J1
R
L
F K
F2 K
Kt Kb
K
N 2
RF2 Kt Kb mgd
R
mgd
1
Dy
D cos y
cosy
J1 J2 N 2 J1 J2
J1 J2 L
L
L
J1
L J1J2 N 2
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.
x x2 1.y
x2 x3 2.y 2.cos y
1
(7)
x3 x4 4.y 5.cos y
x4 x5 6.y 7.cos y
x5 b0.u 8.cos y
y x
1
168
Tr. X. Kiên, B.H. Huế, Tr.Đ.Thuận, “Thiết kế thí nghiệm … rô bốt công nghiệp.”
Nghiên cứu khoa học công nghệ
Ở đó:
là các tham số không xác định tính theo [2].
1 ,2 ,3 ,.....8
R
L
F
F2
Kt K
mgd
J1
1
;
b0
;
3
1
J1 J2
J1J2 NL
R
L
F
F2
Kt Kb
J2L
K
K
F F2
1
1
(8)
;
2
J1 J2
J1 J2 N2 J1J2
R
L
K
K
F F2
F K
F2K Kt Kb F
R
L
F2 mgd
1
1
1
;
5
5
J1 J2 N2 J1J2
J1J2 N2 J1J2
J1J2 L
J2 J1
R
L
F K
F2K
Kt Kb
K
N2
RF2 Kt Kb mgd
R
mgd
1
;
7
;
8
6
J1J2 N2 J1J2
J1J2L
L
L
J1
L J1J2 N2
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 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
y
4
y
T
u c5 .z5 z4 d5 .
.z5 k 5.0,1
0,1 ( y)
02
5
5
p
4
0
4
i
(10)
A . k.y ( y)
A . ( y)
0
0
0
0
i
j
j 0
m
4
vi
4
1
4
yr
yr
A .v
.sgn(b ) .z
i
0
m
1
1
j 0
5
3
2
T .
Và
.
V5
c .z
i
i
4 d i
i 1
Theo lý thuyết LaSelle-Yoshizawa đảm bảo giới hạn toàn cục của:
x(t),0,1 (t),.....,0,5 (t),......0,i (t)......8,1 (t),.....,8,5 (t)
và sai số bám sát limt y(t) y (t) 0 .
r
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.
169
Tạp chí Nghiên cứu KH&CN quân sự, Số Đặc san Viện Điện tử, 10 - 2015
Kỹ thuật điều khiển
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.
3
7
2
1
4
5
6
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.
30
25
20
1
10
0
2
-10
-20
-25
-30
0
5
10
15
20
25
Thoi gian (giay)
30
35
40
45
50
50
40
30
20
10
0
-10
-20
-30
-40
-50
0
5
10
15
20
25
Thoi gian (giay)
30
35
40
45
50
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
170
Tr. X. Kiên, B.H. Huế, Tr.Đ.Thuận, “Thiết kế thí nghiệm … rô bốt công nghiệp.”
Nghiên cứu khoa học công nghệ
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ự.
171
Tạp chí Nghiên cứu KH&CN quân sự, Số Đặc san Viện Điện tử, 10 - 2015
8 trang
09/01/2024
6200
Free
Bạn đang xem tài liệu "Thiết kế thí nghiệm bộ điều khiển thích nghi Observer Backstepping cho rô bốt công nghiệp", để tải tài liệu gốc về máy hãy click vào nút Download ở trên
File đính kèm:
- thiet_ke_thi_nghiem_bo_dieu_khien_thich_nghi_observer_backst.pdf
