Luận án Tổng hợp hệ điều khiển tay máy có khớp đàn hồi ứng dụng cảm biến vi cơ quán tính

1
 NN
 112 2 2
 a24 AiAiax()(), y 25 AiAiAi x ()()(), y z (2.73) 
 NNii11
 NN
 1122
 a26 AiAiAibx()()(), y z 2 g AiAi x ()(), y
 NNii11
 NNN
 12 2 1 4 1 2 2
 a31 AiAiax()(), y 32 Aia y (), 33 AiAi z ()(), y
 NNNi1 i 1 i 1
 NN
 1132
 a34 AiAiax()(), y 35 AiAiAi x ()()(), z y (2.74) 
 NNii11
 NN
 113 2 2
 a36 Ay( i ) A z ( i ), b 3 g A y ( i ),
 NNii11
 NNN
 13 1 2 1 3
 a41 AiAiax()(), z 42 AiAiAia y ()()(), x z 43 AiAi z ()(), x
 NNNi1 i 1 i 1
 NNN
 12 1 2 2 1 2
 a44 AiAiAiax()()(), y z 45 AiAia x ()(), z 46 AiAiAi x ()()(), y z (2.75) 
 NNNi1 i 1 i 1
 N
 1 2
 b4 g Axz( i ) A ( i ),
 N i 1
 NNN
 12 1 3 1 3
 a51 AiAiAiax()()(), y y 52 AiAia y ()(), z 53 AiAi z ()(), y
 NNNi1 i 1 i 1
 NNN
 12 1 2 1 2 2
 a54 AiAiAiax()()(), y z 55 AiAiAia x ()()(), y z 56 AiAi y ()(), z (2.76) 
 NNNi1 i 1 i 1
 N 
 1 2
 b5 g Azy( i ) A ( i ),
 N i 1
 NNN
 12 2 1 2 2 1 4
 a61 AiAiax()(), z 62 AiAia y ()(), z 63 Ai z (),
 NNNi1 i 1 i 1
 NN
 113
 a64 AiAiAiax()()(), y z 65 AiAi x ()(), z (2.77) 
 NNii11
 NN
 113 2 2
 a66 Ay( i ) A z ( i ), b 6 g A z ( i ),
 NNii11
 Việc chia các tổng cho N khi xác định aij ( i 1,6, j 1,6), bi ( i 1,6)không ảnh 
hưởng đến kết quả song nó có giả trị lọc nhiễu ngẫu nhiên dạng ồn trắng của phần tử 
đo. Đặt: 
 2 2 2 2 2 2
 x1( cccx 1242 ),( ccxcx 35364 ), ,2( ccccx 23455 ),2,2 ccx 466 cc 56 (2.78) 
 Với cách đặt (2.72), (2.73), (2.74), (2.75), (2.76), (2.77), (2.78) ta có hệ phương 
trình tuyến tính được viết dưới dạng ma trận như sau: 
 45 
 a11 a 12 a 13 a 14 a 15 a 16 xb11
 a21 a 22 a 23 a 24 a 25 a 26 xb22
 a a a a a a xb
 31 32 33 34 35 36 33 (2.79) 
 a41 a 42 a 43 a 44 a 45 a 46 xb44
 a51 a 52 a 53 a 54 a 55 a 56 xb55
 a61 a 62 a 63 a 64 a 65 a 66 xb66
 Giải hệ phương trình tuyến tính (2.79) bằng các thuật toán trên máy tính nhận được 
các nghiệm x1,,,,, x 2 x 3 x 4 x 5 x 6 và từ biểu thức (2.78) nhận được các tham số hiệu chuẩn 
như sau: 
 c6 x 3, c 5 x 6 /(2), c 6 c 4 x 5 /(2), c 6
 x (2.80) 
 c xcc2 2,()/,4 cccc xcc 2 2
 3 2522 4531 124
 Vì c1,, c 3 c 6 liên quan đến hệ số tỉ lệ, nên luôn là số dương, vì vậy trong các 
biểu (2.80) chỉ lấy phần dương của phép khai căn. Như vậy, đã xác định được đầy đủ 
các tham số hiệu chuẩn. 
 Khi hiệu chuẩn có một số lưu ý sau: 
 - Có thể dùng gia tốc trọng trường để hiệu chuẩn hệ đo gia tốc, tuy nhiên cần 
chú ý: mỗi lần cập nhật (ghi) các giá trị của các gia tốc kế ở các tư thế khác nhau cần 
đặt thiết bị dẫn đường quán tính nằm yên (vì chỉ khi nằm yên nó mới cho chỉ số các 
thành phần gia tốc trọng trường và cần có thông tin về giá trị g đạt độ chính xác rất 
cao). 
 - Có thể dùng véc tơ từ trường trái đất để hiệu chuẩn hệ đo từ trường. Khi cập 
nhật số liệu không cần phải để thiết bị đứng yên. Tuy nhiên, như trên đã phân tích, 
việc hiệu chuẩn phải thực hiện bằng cách đưa cả thiết bị chuyển động ở các tư thế khác 
nhau, vì tham số độ lệch phụ thuộc vào môi trường đặt thiết bị dẫn đường quán tính. 
 - Việc hiệu chuẩn hệ đo thành phần véc tơ vận tốc góc của các con quay vi cơ 
cần phải thực hiện bằng cơ cấu tạo ra vận tốc góc chuẩn và ổn định bậc cao (không 
thay đổi giá trị trong quá trình hiệu chuẩn) và cho chỉ số đạt độ chính xác cao. Có hai 
cách đặt thiết bị khi hiệu chuẩn: 
 + Đặt thiết bị tạo tốc độ quay chuẩn sao cho véc tơ tốc độ góc song song với 
mặt phẳng ngang nơi hiệu chuẩn và quay về hướng đông địa lý. Việc này được thực 
hiện để loại trừ ảnh hưởng sự quay của trái đât. 
 46 
 + Đặt thiết bị tạo tốc độ quay chuẩn sao cho véc tơ tốc độ góc tạo với mặt 
phẳng ngang nơi hiệu chuẩn một góc bắng vĩ độ nơi hiệu chuẩn và quay về hướng bắc 
địa lý, khi đó trong thuật toán phải cộng thêm tốc độ quay trái đất vào độ lớn véc tơ 
tốc độ chuẩn. 
 + Việc thu thập dữ liệu Ax( i ), A y ( i ), A z ( i )cần thực hiện đủ số lượng ở các hướng 
khác nhau. Chỉ có như vậy mới tiệm cận được các giá tri thực c1,,,,, c 2 c 3 c 4 c 5 c 6 . 
 + Việc hiệu chuẩn được thực hiện trong điều kiện nhiệt độ chuẩn cố định. Khi 
lắp đặt trên thiết bị chuyển động các phần tử đo cần đưa vào hộp có cơ cấu ổn định 
nhiệt độ trùng với nhiệt độ khi hiệu chuẩn. 
 2.5. 
 2.4 đã đề cập vấn đề hiệu ch 
sai số hệ số chỉ thị, do độ chệch không và do độ không vuông góc giữa các phương lắp 
đặt các phần tử đo. Ở đó đã chỉ ra hệ tọa độ hiệu chuẩn là hệ tọa độ có một trục trùng 
với phương của một trong 3 phần tử đo véc tơ cần đo (hoặc gia tốc, hoặc tốc độ góc, 
hoặc từ trường trái đất), trục thứ hai nằm trong mặt phẳng chứa hai phương của hai 
phần tử đo (một phương trùng với phương của trục thứ nhất), trục thứ ba tạo với hai 
trục đã chọn một tam diện thuận. Hệ tọa độ này được gọi là hệ tọa độ cơ sở. Do trong 
thực tế, ta không thể lắp đặt hệ tọa độ cơ sở trùng với hệ tọa độ liên kết đã chọn. Vì 
vậy khi xây dựng thuật toán xác định tham số ần phải có 
thông tin về độ lệch giữa hai toạ độ này. 
 Giả sử hệ tọa độ cơ sở và hệ tọa độ liên kết của vật thể chuyển động có độ 
chệch nhau (hình 2.8). 
 Y
 Y’
 X’
 X
 Z
 Z’ 
 Hình 2.8. Hệ tọa độ cơ sở và hệ toạ độ liên kết. 
 47 
 Vì hai hệ tọa độ đều là hệ tọa độ Đề các nên tồn tại ma trận cô sin chỉ phương thể 
hiện quan hệ giữa hai hệ tọa độ: 
 1
 a11 a 12 a 13 c 11 c 12 c 13
 1 T
 A a21 a 22 a 23 C C c 21 c 22 c 23 (2.81) 
 a31 a 32 a 33 c 31 c 32 c 33
 Vấn đề hiệu chỉnh chính là vấn đề từ 3 chỉ số đo 3 thành phần của véc tơ cần đo 
trong hệ tọa độ cơ sở cần phải xác định các số đo đó trong hệ tọa độ liên kết. Giả sử 
chỉ số của các phần tử đo là (,,)1 2 3 thì khi đó các số đo trong hệ tọa độ liên kết sẽ 
là (,,)1 2 3 và được xác định như sau: 
 11
 22C (2.82) 
 33
 Như vậy, vấn đề hiệu chỉnh thực chất là vấn đề xác định ma trận A hoặc ma 
trận C . 
 2.5.1. Phương pháp hiệu chỉnh 6 vị trí. 
 Từ tính chất của ma trận cô sin chỉ phương [41]: 
 AA11 AATT AC CC CC CA I (2.83) 
 (trong đó I là ma trận đơn vị) ta có: 
 ccc11 12 13 ccc 11 21 31 ccc 11 21 31 ccc 11 12 13 1 0 0
 ccc21 22 23 ccc 12 22 32 ccc 12 22 32 ccc 21 22 23 0 1 0 (2.84) 
 ccc31 32 33 ccc 13 23 33 ccc 13 23 33 ccc 31 32 33 0 0 1
 Triển khai (2.84) sẽ có 3 phương trình sau: 
 2 2 2
 c11 c 21 c 31 1 (2.85) 
 c11 c 12 c 21 c 22 c 31 c 32 0 (2.86) 
 c11 c 13 c 21 c 23 c 31 c 33 0 (2.87) 
 Từ (2.84), (2.85), (2.86) cho thấy nếu bằng cách nào đó xác định được 6 tham số 
 c21,,,,, c 22 c 23 c 31 c 32 c 33 
thì có thể xác định 3 tham số còn lại như sau: 
 22
 c111 c 21 c 31 (2.88) 
 c12()/ c 21 c 22 c 31 c 32 c 11 (2.89) 
 c13()/ c 21 c 23 c 31 c 33 c 11 (2.90) 
 48 
 Vì c11 0 nên trong biểu thức (2.88) chỉ lấy phần dương trước dấu căn. Triển khai 
(2.82) có: 
 1c 11 1 c 12 2 c 13 3 (2.91) 
 2c 21 1 c 22 2 c 23 3 (2.92) 
 3c 31 1 c 32 2 c 33 3 (2.93) 
 Từ hệ phương trình (2.91), (2.92) dễ dàng nhận thấy nếu bằng cách 
nào đó tạo ra 6 lần thực nghiệm với 6 vị trí khác nhau và bằng cách nào đó có được 
các giá trị 23, một cách chính xác và đọc các chỉ số 1,, 2 3 . Sau đó lập 6 phương 
trình tuyến tính dạng (2.92), (2.93) và coi c21,,,,, c 22 c 23 c 31 c 32 c 33 là nghiệm, chúng ta sẽ 
nhận được hệ phương trình tuyến tính 6 ẩn với 6 phương trình. Dùng thuật toán giải hệ 
phương trình tuyến tính sẽ nhận được . Còn 3 tham số c11,, c 12 c 13 
được xác định theo các biểu thức (2.88), (2.89), (2.90). Các vị trí được thực hiện khi 
hiệu chỉnh thể hiện trên hình 2.9. 
 Ay Ax 
 Ay Ax 
 Ay 
 Ax 
 Vị trí I Vị trí II Vị trí III 
 Ax Az Ax Ax 
 A 
 Ay z
 Vị trí IV Vị trí V Vị trí VI 
 Hình 2.9 vị trí cần có để xác định các hệ số hiệu chuẩn 
 Trong trường hợp hiệu chỉnh hệ đo véc tơ gia tốc được thực hiện như sau: Giả sử 
hệ đo gia tốc gồm 3 gia tốc kế được đặt trên một giá đo gồm 3 trục được căn chỉnh 
vuông góc từng cặp đạt độ chính xác cao, sau đó quay giá đo đó và đặt nó ở 6 vị trí 
như hình 2.9 
 Lấy số liệu đo của các gia tốc kế ứng với 6 vị trí (tương ứng với ). Giá 
trị ay, az (tương ứng với ) ở vị trí sẽ có giá trị như sau: 
 49 
 vị trí I: (-g, 0); vị trí II: (0, 0); vị trí III: ( g 2 / 2,- ) ... 
 Dễ dàng nhận thấy, phương pháp hiệu chuẩn này có ưu điểm cơ bản là đơn giản 
trong việc giải hệ phương trình (giải hệ phương trình tuyến tính) xác định các tham số 
hiệu chỉnh. Thao tác vận hành cũng giản đơn. Song phương pháp trên có các nhược 
điểm và khi thực hiện cần lưu ý sau: 
 - Cần phải có giá dùng để gá thiết bị đo quán tính có độ vuông góc rất cao. 
 - Phải có cơ cấu quay giá đỡ vào 6 vị trí 0o, 45o, 90o, 135o độ chính xác cao. 
 - Cần phải đo giá trị g ở vị trí hiệu chuẩn đạt độ chính xác cao. 
 Ngoài ra các chỉ số đo của các gia tốc kế đều có nhiễu đo dạng ồn trắng. 
 Do vậy, cần phải có thuật toán lọc tín hiệu này trước khi đưa vào 6 hệ phương 
trình. 
 2.5.2. Phương pháp hiệu chỉnh cho hệ đo véc tơ tốc độ góc 
 Giả sử hệ đo véc tơ tốc độ góc gồm 3 con quay vi cơ đo 3 thành phần đã được 
 ddd
hiệu chuẩn theo thuật toán chương 2, khi đó có 3 số đo x,, y z . Từ 3 số đo này nếu 
biết ma trận C có thể xác định chỉ số thành phần của véc tơ tốc độ góc trong hệ tọa độ 
liên kết như sau: 
 d d d
 xc11 x c 12 y c 13 z (2.94) 
 d d d
 yc21 x c 22 y c 23 z (2.95) 
 d d d
 zc31 x c 32 y c 33 z (2.96) 
 Khi có thông tin x,, y z có thể xác định các góc định hướng trong hệ tọa độ 
dẫn đường nào đó bằng cách giải hệ phương trình vi phân: 
 yzsin cos (2.97) 
 1
 (co s sin ) (2.98) 
 cos yz
 xtg( y co s z sin ) (2.99) 
 Thay giá trị yz, theo (2.95), (2.96) vào (2.97), (2.98) có: 
 d d d d d d
 (c21x c 22 y c 23 z )sin ( c 31 x c 32 y c 33 z ) cos (2.100) 
 cos sin
 ()()cd c d c d c d c d c d (2.101) 
 21x 22 y 23 zcos 31 x 32 y 33 z cos
 50 
 Giả sử hệ đo được gá trên giá quay ba bậc tự do và được quay với các vận tốc 
khác nhau và có các cơ cấu đo góc ,, . Khi đó có thể dựa vào thông tin về các 
 ddd
góc ,,do giá quay cấp và các giá trị đo x,, y z do các con quay vi cơ cấp có 
thể xác định các tham số . Giả sử giá quay gá hệ đo được quay lần 
1 từ thời gian bắt đầu đến khi kết thúc là T . Tiến hành tích phân hai vế hai phương 
trình (2.100), (2.101) có: 
 TTT
 d d d
 (T ) (0) c21x sin dt c 22 y sin dt c 23 z sin dt
 0 0 0 (2.102) 
 TTT
 d d d 
 c cos dt c co s dt c co s dt
 31xc21 32,,,,, c 22 y c 23 c 31 c 32 33c 33 z
 0 0 0
 TTT
 cosd cos d cos d
 (T ) (0) c21x dt c 22 y dt c 23 z dt
 0cos 0 cos 0 cos ( 2.103) 
 TTT
 sind sin dcos d
 ()c31x dt c 32 y dt c 33 z dt
 0cos 0 cos 0 cos
 Chia khoảng thời gian thành N khoảng nhỏ có giá trị T/N và thay các tích 
phân bằng các tổng sẽ có hai phương trình tương đương: 
 NN
 TTdd
 (T ) (0) c21xy ( i )sin ( i ) c 22 ( i )sin ( i )
 NNii11
 NN
 TTdd
 c23zx( i )sin ( i ) c 31 ( i ) co s ( i ) (2.104) 
 NNii11
 NN
 TTdd
 c32yz( i ) co s ( i ) c 33 ( i ) co s ( i )
 NNii11
 Các tổng trong các biểu thức (2.104) là các số cụ thể nên được ký hiệu như sau: 
 NNN
 T cos()()() id cos i d cos i d
 ()(0)T c21x () i c 22 y () i c 23 z () i
 Ni1 cos()()() i i 1 cos i i 1 cos i
 NNN (2.105) 
 sin (i )d sin ( i ) d sin ( i ) d
 (()()())c31x i c 32 y i c 33 z i
 i1cos()()() i i 1 cos i i 1 cos i
 N N N
 T d T d T d
 d11 x ( i )sin ( i ), d12 y ( i )sin ( i ) , d13 z ( i )sin ( i ) (2.106) 
 N i 1 N i 1 N i 1
 N N N
 T d T d T d
 d14 x ( i ) co s ( i ) , d15 y ( i ) co s ( i ) , d16 z ( i ) co s ( i ) , (2.107) 
 N i 1 N i 1 N i 1
 51 
 N N N
 T cos() i d T cos() i d T cos() i d 
di21 x (), di22 y (), di23 z () 
 Ni 1 cos() i Ni 1 cos() i Ni 1 cos() i (2.108) 
 N N N
 Tisin ( ) d Tisin ( ) d Tisin ( ) d
di24 x ( ), di25 y ( ), di26 z () (2.109) 
 Ni 1 cos() i Ni 1 cos() i Ni 1 cos() i
 Với các ký hiệu như (2.106), (2.107), (2.108), (2.109) và đặt: 
 T
xcxcxcxcxcxcb1 212, 223 , 234 , 315 , 326 , 331 ; ()(0), T b 2 ()(0) T (2.110) 
khi đó hai phương trình (2.104), (2.105) sẽ có dạng: 
 dxdx11 1 12 2 dx 13 3 dx 14 4 dx 15 5 dx 16 6 b 1 (2.111) 
 dxdx21 1 22 2 dxdx 23 3 24 4 dxdx 25 5 26 6 b 2 (2.112) 
 Tiếp tục tiến hành cho giá quay hoạt động trong khoảng thời gian từ đến 2T sẽ 
nhận được: 
 dxdx31 1 32 2 dxdx 33 3 34 4 dx 35 5 dx 36 6 b 3 (2.113) 
 dxdx41 1 42 2 dxdx 43 3 44 4 dxdx 45 5 46 6 b 4 (2.114) 
trong đó: 
 2N 2N 2N
 T d T d T d
 d31 x ( i )sin ( i ) , d32 y ( i )sin ( i ) , d33 z ( i )sin ( i ) (2.115) 
 N iN1 N iN1 N iN1
 2N 2N 2N
 T d T d T d
 d34 x ( i ) co s ( i ) , d35 y ( i ) co s ( i ) , d36 z ( i ) co s ( i ) (2.116) 
 N iN1 N iN1 N iN1
 2NNN 2 2
 T cos()()() id T cos i d T cos i d
d41x( i ), d 42 y ( i ), d 43 z ( i ) (2.117) 
 Ni N1 cos()()() i N i N 1 cos i N i N 1 cos i
 2NNN 2 2
 Tsin ( i )d T sin ( i ) d T sin ( i ) d
d44x( i ), d 45 y ( i ), d 46 z ( i ) (2.118) 
 Ni N1 cos()()() i N i N 1 cos i N i N 1 cos i
 b34(2) T (), T b (2) T () T 2.119) 
Tiếp tục tiến hành cho giá quay hoạt động trong khoảng thời gian từ 2T đến 3T sẽ 
nhận được: 
 dxdx51 1 52 2 dxdx 53 3 54 4 dx 55 5 dx 56 6 b 5 (2.120) 
 dxdx61 1 62 2 dxdx 63 3 64 4 dxdx 65 5 66 6 b 6 (2.121) 
 trong đó: 
 3N 3N 3N
 T d T d T d
d51 x ( i )sin ( i ) , d52 y ( i )sin ( i ) , d53 z ( i )sin ( i ) (2.122) 
 N iN21 N iN21 N iN21
 52 
 3N 3N 3N
 T d T d T d
 d54 x ( i ) co s ( i ) , d55 y ( i ) co s ( i ) , d56 z ( i ) co s ( i ) (2.123) 
 N iN21 N iN21 N iN21
 3N 3N 3N
 T cos() i d T cos() i d T cos() i d
 di61 x (), di62 y (), di63 z () (2.124) 
 NiN21 cos() i NiN21 cos() i NiN21 cos() i
 3N 3N
 Tisin ( ) d Tisin ( ) d
 di64 x (), di65 y ( ),
 NiN21 cos() i NiN21 cos() i (2.125) 
 3N
 Tisin ( ) d 
 di66 z () 
 NiN21 cos() i
 b56(3) T (2), T b (3) T (2) T 2.126) 
 Sáu phương trình (2.111), (2.112), (2.113), (2.114), (2.120), (2.121) là 6 phương 
 c21,,,,, c 22 c 23 c 31 c 32 c 33
trình tuyến tính với 6 ẩn. Giải hệ phương trình này sẽ cho các nghiệm là các tham số 
hiệu chỉnh cần tìm. Còn 3 tham số được xác định theo 
 c11,, c 12 c 13
các biểu thức (2.88), (2.89), (2.90). Như vậy đã xác định đầy đủ 9 phần tử của ma trận 
cosin chỉ phương giữa hệ tọa độ đo và hệ tọa độ liên kết của vật thể chuyển động. 
 2.5.3. Phương pháp hiệu chỉnh cho hệ đo véc tơ gia tốc 
 C
 Giả sử hệ đo véc tơ gia tốc gồm 3 gia tốc kế đo 3 thành phần đã được hiệu 
 ddd
chuẩn theo thuật toán chương 2, khi đó có 3 số đo AAAx,, y z . Từ 3 số đo này nếu biết 
ma trận có thể xác định chỉ số thành phần của véc tơ gia tốc trong hệ tọa độ liên kết 
như sau: 
 d d d
 Ax c11 A x c 12 A y c 13 A z (2.127) 
 d d d
 Ay c21 A x c 22 A y c 23 A z (2.128) 
 d d d
 Az c31 A x c 32 A y c 33 A z (2.129) 
 Cho chuyển động trên một 
đường ray chuyên dụng sao cho trong quá trình chuyển động vật thể (hoặc thiết bị dẫn 
đường quán tính) không quay. Trên ray gắn thước đo điện tử để xác định vận tốc vật 
thể chuyển động. Giả sử cho vật thể chuyển động thẳng theo phương trùng với phương 
trục OX của hệ tọa độ liên kết trong mặt phẳng nằm ngang. Khi đó, AAyz0, 0 (vật 
không chuyển động theo hai phương này). Phương trình mô tả chuyển động sẽ là: 
 dV
 x A c Ad c A d c A d (2.130) 
 dt x11 x 12 y 13 z
hoặc: 
 53 
 d d d
 dVx() c11 A x c 12 A y c 13 A z dt (2.131) 
Cho thiết bị chuyển động có gia tốc trên ray trong khoảng thời gian từ 0 đến T. Tiến 
hành tích phân hai vế phương trình (2.131): 
 TTTT
 d d d
 dVx VT x( ) V x (0) c11 Adtc x 12 Adtc y 13 Adt z (2.132) 
 0 0T 0 0
Chia khoảng thời gian thành khoảng nhỏ có giá trị T/N và thay các tích phân ở 
vế phải biểu thức (2.132) bằng các tổng sẽ có phương trình tương đương: 
 NNN
 TTTd d d
 VTVx()(0) x c11 Aic x () 12 Aic y () 13 Ai z () (2.133) 
 NNNi1 i 1 i 1
Tiếp tục cho thiết bị chuyển động có gia tốc trong khoảng thời gian từ đến 2T sẽ có 
phương trình: 
 2NNN 2 2
 TTTd d d
 VTVTcx(2)() x11 Aic x () 12 Aic y () 13 Ai z () (2.134) 
 NNNi N1 i N 1 i N 1
Tiếp tục cho thiết bị chuyển động có gia tốc trong khoảng thời gian từ 2T đến 3T sẽ 
có phương trình: 
 3NNN 3 3
 TTTdN d d
 VTVTcx(3)(2) x11 Aic x () 12 Aic y () 13 Ai z () (2.135) 
 NNNi2 N 1 i 2 N 1 i 2 N 1
Đặt: 
 xcxcxcbVTV1112123131, , ,x () x (0), bVTVTbVTVT 2 x (2) x (), 3 x (3) x (2) (2.136) 
 Với cách đặt như (2.136), viết ba phương trình (2.133), (2.134), (2.135) dưới dạng 
ma trận như sau: 
 TTTNNN
 Addd()()() i A i A i
 NNNx y z
 iii111xb
 2NNN 2 2 11
 TTTd d d
 Ax()()() i A y i A z i x22 b (2.137) 
 NNNi N1 i N 1 i N 1
 3NNN 3 3 xb33
 TTTd d d
 Ax()()() i A y i A z i
 NNNi2 N 1 i 2 N 1 i 2 N 1
 Giải hệ phương trình tuyến tính (2.137) sẽ cho nghiệm là các tham số c11,, c 12 c 13 . 
Tiếp tục cho chuyển động tịnh tiến có gia tốc theo phương 
trùng với trục OY của hệ tọa độ liên kết trong mặt phẳng nằm ngang. Thực hiện các 
thủ tục như khi chuyển động theo phương OX sẽ nhận được các tham số c21,, c 22 c 23 . 
Các tham số c31,, c 32 c 33 được tính theo các biểu thức dạng (2.88), (2.89), (2.90). 
 54 
 Xét ma trận trong hệ phương trình (2.137): 
 NNN
 TTTddd
 Ax()()() i A y i A z i
 NNNiii111
 2NNN 2 2
 TTTd d d
 D Ax()()() i A y i A z i (2.138) 
 NNNi N1 i N 1 i N 1
 3NNN 3 3
 TTTd d d
 Ax()()() i A y i A z i
 NNNi2 N 1 i 2 N 1 i 2 N 1
Dễ dàng nhận thấy để ma trận D không suy biến (định thức khác không) th gia tốc ở 
3 giai đoạn phải khác nhau. 
 chƣơng 2 
Từ các kết quả đã nêu cho phép rút ra các kết luận sau: 
 1. Có thể ứng dụng các phần tử đo quán tính (con quay vi cơ, gia tốc kế) có độ 
trôi và nhiễu đo kết hợp với các phần tử đo phi quán tính (từ kế) để đo các vị trí góc 
của vật thể chuyển động nói chung và trong tay máy nói riêng nhờ áp dụng thuật toán 
lọc Kalman phi tuyến mở rộng. Trong trường hợp không có thông tin từ các từ ó 
thể xác định các tham số góc bằng giải pháp nhận dạng độ trôi con quay vi cơ khi tay 
máy đứng yên và giải trực tiếp các phương trình vi phân để xác định tham số góc khi 
tay máy chuyển động. 
 2. Việc áp dụng các phần tử đo quán tính cũng như phi quán tính đòi hỏi phải 
có thuật toán hiệu chỉnh do sai số lắp đặt và sai số chế tạo. Việc xây dựng thuật toán 
cho phép nâng cao độ chính xác trong điều kiện sử dụng các phần tử đo thông dụng 
 trên thị trường. 
 3. Việc lắp đặt các thiết bị đo đã hiệu chỉnh vào các vật chuyển động nói chung 
và vào tay máy nói riêng đòi hỏi phải hiệu chuẩn đồng bộ hóa hệ tọa độ của vật thể và 
hệ đo. Thuật toán đã giải quyết vấn đề này. 
 Các thuật toán nêu ra trong chương này là cơ sở để sử dụng các phần tử quán 
tính cũng như phi quán tính để xác định tư thế của vật thể nói chung và của 
nói riêng. 
 55 
 Chƣơng 3: 
 XÂY DỰNG THUẬT TOÁN ĐIỀU KHIỂN CHO TAY MÁY 
 3.1. Cơ 
 uzzy), nơ ron (Neural)... một phương 
pháp mới được phát triển để thiết kế bộ điều khiển hệ thống động học phi tuyến, đó là 
phương pháp . Phương pháp dựa trên thủ tục đệ qui, thiết kế ổn 
định toàn cục hệ thống điều khiển, có cấu trúc phân cấp 
 – . Phương pháp dựa trên mô hình toán 
học của hệ thống khả lý thuyết hàm Lyapunov
 u y
 Xn Xn-1 X2 X1
 3.1.1. Tính ổn định Lyapunov 
 ến có mô hình thay đổi theo thời gian, còn gọi là hệ không dừng: 
 x f(,) x t (3.1) 
 Tn
Với x( x1 ,... xn ) c tơ gồm n biến trạng thái của hệ, f(,) x t là của n các 
 T
hàm thực f( x , t ) ( f1 ( x , t ),..., fn ( x , t )) thường được giả thiết là liên tục theo t và thỏa 
mãn điều kiện Lipschitz địa phương [10] tại lân cận x0 
 Giả thiết tiếp hệ (3.1) là cân bằng tại gốc tọa độ, tức là: 
 ft(0, ) 0 với t (3.2) 
 56 
 Trong đó 0 (0,...,0)T là vector không của không gian vector n. Giả thiết (3.2) 
này nói rằng khi không có tác động từ bên ngoài và hệ đang ở trạng thái 0 thì nó sẽ vẫn 
ở nguyên trạng thái đó. Ta định nghĩa về tính ổn định Lyapunov [10] như sau: 
 Định nghĩa 3.1: 
 Xét hệ phi tuyến bậc n (3.1), tự trị, không dừng, cân bằng tại gốc tọa độ 0, tức là 
thỏa mãn (3.2), trong đó vector hàm f(x,t) được giả thiết là liên tục theo t. Khi đó hệ sẽ 
được gọi là: 
 a) Ổn định, nếu với mọi hằng số thực dương ε > 0 t0 > 0 cho trước luôn tồn tại 
 ( ,t0 ) 0 phụ thuộc ε và t0 sao cho: 
 x0(,),, t 0 x t x 0 t 0 với mọi t 0 (3.3) 
 Trong đó x()(,,) t x t x00 t là nghiệm củ