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

Xây dựng thuật toán xác định tham số góc cho tay máy ứng dụng các phần tử vi cơ trong trường hợ ố .... MỞ ĐẦU Trong quá trình công nghiệp hoá, hiện đại hoá đất nước, các ngành công nghiệ

BỘ GIÁO DỤC VÀ ĐÀO TẠO BỘ QUỐC PHÒNG

VIỆN KHOA HỌC VÀ CÔNG NGHỆ QUÂN SỰ

TỔNG HỢP HỆ ĐIỀU KHIỂN TAY MÁY CÓ KHỚP ĐÀN HỒI

ỨNG DỤ

- NĂM 2016

BỘ GIÁO DỤC VÀ ĐÀO TẠO BỘ QUỐC PHÒNG

VIỆN KHOA HỌC VÀ CÔNG NGHỆ QUÂN SỰ

TỔNG HỢP HỆ ĐIỀU KHIỂN TAY MÁY CÓ KHỚP ĐÀN HỒI

Tôi cam đoan đâ tôi

vi

ix

ầu 1

Chương 1: TỔNG QUAN VỀ 5

1.1 ệp 5

1.1.1 Sơ lượ ệp 5

1.1.2 y 6

1.1.3 10

1.2 11

1.3 Tình hình nghiên cứu 14

1.4 ều khiển ch 17

20

Chương 2: XÂY DỰNG THUẬT TOÁN XÁC ĐỊNH TƯ THẾ GÓC VÀ CÁC THUẬT TOÁN HIỆU CHỈNH KHI SỬ DỤNG CÁC VI CƠ QUÁN TÍNH 21

2.1 Giới thiệu thuật toán lọc Kalman 21

2.2 Xây dựng thuật toán xác đị ứng dụng các phần tử ố 22

2.2.1 Ma trận cosin chỉ phương và các góc định hướng 22

2.2.2 Xây dựng thuật toán xác định tham s 27

2.3 Xây dựng thuật toán xác định tham số góc cho tay máy ứng dụng các phần tử vi cơ trong trường hợ ố 32

2.4 Xây dựng thuật toán hiệu các thông số đo 34

2.4 34

2.4.2 Phương pháp và thuật toán hiệu chuẩn 37

2.5 Xây dựng thuật toán nhận dạng hiệu ch hệ đo 46

2.5.1 Phương pháp hiệu chỉnh 6 vị trí 47

2.5.2 Phương pháp hiệu chỉnh cho hệ đo véc tơ tốc độ góc 49

2.5.3 Phương pháp hiệu chỉnh cho hệ đo véc tơ gia tốc 52

Kết luận chương 2 54

Chương 3: XÂY DỰNG THUẬT TOÁN ĐIỀU KHIỂN CHO TAY MÁY

55

3.1 Cơ 55

3.1.1 Tính ổn định Lyapunov 55

3.1.2 Hàm điều khiển Lyapunov 57

3.2 Hệ thống phản hồ 58

61

3.4 Tổng hợp bộ điều khiển cho tay rô bốt 65

3.4.1 Mô hình đối tượng 65

3.4.2 Thiết kế bộ điều khiển 66

Kết luận chương 3 80

Chương 4: ỀU KHIỂN 81

4.1 Mô ậ 81

4.1.1 Mô phỏng kiểm nghiệm thuậ 81

4.1.2 83

4.1.3 85

4.2 87

4.2.1 87

4.2.2 90

Kết luận chương 4 98

100

101

103 P1

DANH MỤC CÁC CÁC KÝ HIỆ CHỮ VIẾT TẮT

PI Bộ điểu khiển tỷ lệ, tích phân (Proportional Integral)

PID Bộ điểu khiển tỷ lệ, tích phân và đạo hàm (Proportional Integral

Derivative)

ệ toạ độ liên kết và hệ toạ độ chuẩn

55

59

a Kết quả thử nghiệm khi quay đối tượng trong không gian 89

b Kết quả sai lệch khi quay vật thể trong mặt phẳng ngang 89

MỞ ĐẦU

Trong quá trình công nghiệp hoá, hiện đại hoá đất nước, các ngành công nghiệp đang phát triển hết sức nhanh chóng, nhiều nhà máy xí nghiệp được xây dựng với quy mô và công nghệ hiện đại, tiên tiến đáp ứng được nhu cầu của tình hình sản xuất hiện nay, trong đó phải kể đến sự tiến bộ vượt bậc của khoa học kỹ thuật, nhất là sự ra đời của máy tính và công nghệ thông tin đã tạo tiền đề cho sự phát triển mạnh mẽ của nền sản xuất có tính chất tự động hoá cao, đã dần thay thế sức lao động của con người đồng thời hiệu quả của nó đem lại cho nền kinh tế là rất lớn

Hệ thống nhận dạng và điều khiển đối tượng truyền động có yếu tố phi tuyế

điều khiển tượng truyền động có yếu tố phi tuyến

:

Kết quả nghiên cứu của luận án là cơ sở khoa họ

ựng phần mềm cho hệ thống điều khiể

ể cải tiến, hiện đạ

truyền

thiết bị đo quán tính vi cơ điện tử cho vấn đề xác đ nh tham số định hướng của tay

Chương 1: Tổng quan về

việc ứng dụng các thiết bị đo quán tính vi cơ điện tử cho vấn đề xác đ nh tham số định hướng của tay máy

2,

Chương 2: Xây dựng thuật toán xác định tư thế góc và các thuật toán

, hiệu chỉnh khi sử dụng các vi cơ quán tính

ệu chuẩn và hiệu chỉnh hệ đo qụán

cũng như phi quán tính sử dụng trong thực tế Thuật toán hiệu chuẩn dùng

để khắc phục các sai số không thể tránh khi chế tạo các thiết bị đo quán tính và phi quán tính Thuật toán hiệu chỉnh dùng để khắc phục các sai số p đặt thiết bị

đo quán tính cũng như phi quán tính vào vật thể chuyển động Việc xây dự ật

, cơ cấu dịch chuyển cơ khí

2, 4, 5

Chương 3 Xây dựng thuật toán điều khiển khớp

y thủ tục thiết kế bộ điều khiển

được đo và là tín hiệu phản hồi duy nhất trong vòng điều khiển, ở đó phần phi tuyến chỉ phụ thuộc vào các tín hiệu đầu ra.Với hệ thống này, luận án xây dựng bộ quan sát phi tuyến dạng hàm mũ hội tụ và thay thế trạng thái không đo được bởi giá trị ước lượng của chúng Thủ tục thiết kế bộ điều khiển dựa theo định lý Lyapunov

sẽ thực hiện song hành cùng việc tính toán bộ quan sát để ước lượng các trạng thái không đo lường được của hệ Các tham số thay đổi của hệ cũng được tính toán, ước lượng bởi các luật cập nhật, đảm bảo tính thích nghi tham số cho hệ có tham số thay đổi

3 10

,,

ều khiể

3, 9

Chương 1:

TỔNG QUAN VỀ

Thuật ngữ “ ” xuất phát từ tiếng Sec (Czech) “Robota” có nghĩa là công việc tạp dịch trong vở kịch “Rossum’s Universal Robots của Karel Capek” vào năm

1921 Trong vở kịch này, Rossum và con trai của ông ta đã chế tạo ra những chiếc máy gần giống với con người để phục vụ con người [4] Có lẽ đó là một gợi ý ban đầu cho các nhà sáng chế kỹ thuật về những cơ cấu, máy móc bắt chước các hoạt động cơ bắp của con người

Đầu thập kỷ 60, công ty Mỹ AMF (American Machine and Foundry Company) quảng cáo một loại máy tự động vạn năng và gọi là “Người máy công nghiệp” (Industrial Robot) Ngày nay người ta đặt tên người máy công nghiệp (hay công nghiệp) cho những loại thiết bị có dáng dấp và một vài chức năng như tay người được điều khiển tự động để thực hiện một số thao tác sản xuất

Về mặt kỹ thuật, những công nghiệp ngày nay, có nguồn gốc từ hai lĩnh vực kỹ thuật ra đời sớm hơn đó là các cơ cấu điều khiển từ xa và các máy công cụ điều khiển số Các cơ cấu điều khiển từ xa thay thế cho cánh tay của người thao tácnhìn chung việc sử dụ ở các nước công nghiệp phát triển như: Nhật, Mỹ, Đức, Italy, Pháp, Anh là rất phổ biến và tốc độ phát triển hằng năm rất nhanh [6] Trong mỗi lĩnh vực nêu trên sự phân bố đối với mỗi loại hình công việc cũng rất khác nhau Ví dụ chỉ riêng trong lĩnh vực đóng gói và lắp ráp người ta thấy ở các nước như Nhật Bản, Đức, Anh cứ 1000 công nhân lắp ráp có khoảng hơn 250 chiếm khoảng 25% Trong lĩnh vực lắp ráp điện tử chiếm tới khoảng 40% Nếu tính chung trong lĩnh vực lắp ráp cơ khí hiện nay r đã chiếm tới gần 50% [9]

Ngày nay, r được dùng hầu hết trong các ngành công nghiệp, đặc biệt trong những ngành có môi trường làm việc độc hại

như: lắ

- Chức năng cảm nhận của r gồm thu nhận tín hiệu về trạng thái môi trường và trạng thái của bản thân hệ thống, do các cảm biến (sensor) và các thiết bị khác đảm nhiệm Hệ thống này gọi là hệ thống thu nhận và xử lý số liệu hay hệ thống cảm biến

1.1.2 Mô hình động học

1.1.2.1 Ma trận quan hệ

b t được coi là một tay máy có một vài bậc tự do có thể coi là một tập hợp các khâu gắn liền với các khớp [13]

OX0Y0Z0 Sử dụng các phép biến đổi thuần nhất có thể mô tả

vị trí tương đối và hướng giữa các hệ toạ độ này A 1 mô tả vị trí và hướng

của khâu đầu tiên, A 2 mô tả vị trí và hướng của khâu thứ hai so với khâu thứ nhất vị trí và hướng của khâu thứ hai so với hệ toạ độ gốc được biểu diễn bởi ma trận [9]:

thuần nhất 4x4} để mô tả quan hệ giữa 2 khâu liên tiếp trong cơ cấu không gian

1.1.2.2 Bộ thông số Denavit-Hartenberg (DH)

Một nhiều khâu cấu thành từ các khâu nối tiếp nhau thông qua các khớp

Khâu “1“ nối với khâu chuẩn bởi khớp “1“ và không có khớp ở đầu mút của khâu cuối cùng Bất kỳ khâu nào cũng được đặc trưng bởi hai kích thước [4]:

- Độ dài pháp tuyến chung: an

- Góc giữa các trục trong mặt phẳng vuông góc với an: αn

Thông thường, người ta gọi an là chiều dài và αn là góc xoắn của khâu (hình 1.3) Phổ biến là hai khâu liên kết với nhau ở chính trục của khớp (hình 1.4)

Khâu i

Khớp i+1Khớp i

- a i là độ dài vuông góc chung giữa hai trục khớp động i+1 và i

- i là góc chéo giữa hai trục khớp động i+1 và i

- d i khoảng cách đo dọc trục khớp động i từ đường vuông góc chung giữa trục khớp động i+1 và trục khớp động n tới đường vuông góc chung giữa khớp động i

và trục khớp động i-1

- i là góc giữa hai đường vuông góc chung nói trên

Thông số a i , α i , d i và θ i được gọi là bộ thông số Denavit – Hartenberg (DH) ếu

khớp động i là khớp quay thì i là biến khớp ếu khớp động i là tịnh tiến thì d i là biến khớp

hiết lập hệ tọa độ như sau:

ta gắn vào mỗi khâu một hệ ộ Nguyên tắc chung để gắn hệ tọa độ lên các khâu như sau:

- Gốc của hệ ộ gắn lên khâu thứ i đặt tại giao điểm của pháp tuyến a i với trục

khớp thứ i+1

- Trục Z của hệ ộ gắn lên khâu thứ n đặt dọc theo trục khớp thứ i+1

- Trục X thường được đặt dọc theo pháp tuyến chung và hướng từ khớp i đến i+1

Trong trường hợp các trục khớp cắt nhau thì trục X chọn theo tích v tơ Z i Z i-1

Trường hợp khớp quay thì θ i là các biến khớp, trong trường hợp khớp tịnh tiến thì

1.5

Z0O

Hệ tọa độ cố định là OX0Y0Z0, hệ tọa độ O1X1Y1Z1 có gốc O1 đặt tại tâm trục

khớp động 1, hệ tọa độ O2X2Y2Z2 có gốc O2 đặt tại tâm trục khớp động cuối khâu 2

-Tay máy là một bộ phận cơ khí quan trọ ủ

ều bậc tự do, có khả năng thực hiện các thao tác trong mặt phẳng hoặc trong không gian với một phạm vi nhất

ẫn động

ều khiểđ

EE

Encoder

Encoder ( 2c)

1.2

như trêng

Y

X

q

q2I

ΔqII

Để chế tạo r thường phải giải quyết các vấn đề cơ bản sau: Vấn đề chế tạo

và gia công cơ khí, vấn đề đo lường và điều khiển

Việc chế tạo và gia công cơ khí là một vấn đề cơ bản khi chế tạo Tùy theo chức năng và nhiệm vụ của mà có các cấu hình cơ khác nhau Những

có sự dịch chuyển vị trí người ta có thể chế tạo các với các dạng vận động khác nhau, có thể vận động mô phỏng chuyển động của động vật hoặc dạng chuyển

một không gian cố định người ta thường chế tạo dạng tay máy, m thao tác cánh tay của con người Trong luận án này giới hạn nghiên cứu vấn đề đo lường và điều khiển cho tay máy mà không xem xét vấn đề gia công cơ khí

Vấn đề đo lường và điều khiển là vấn đề mang tính cơ bản khi thiết kế chế tạo

trong tay máy, gồm các tham số định hướng, vận tốc góc cùng gia tốc của chúng Vấn đề điều khiển thực chất là vấn đề điều khiển chuyển động của các khớp trong tay máy sao cho đạt yêu cầu về thao tác mà tay máy phải đảm nhận

Hai vấn đề nêu trên luôn luôn được giải quyết khi thực hiện chế tạo tay máy nói chung và hệ thống điều khiển tay máy nói riêng

Cùng với sự ra đời của các phương tiện đo khác nhau là các phương pháp khác

máy nói chung Đối với tay máy để đo các tham số dịch chuyển (vị trí góc, tốc độ góc,

vị trí tốc độ ) người ta luôn luôn áp dụng các phương tiện đo hợp lý, phù hợp với từng giai đoạn phát triển các kỹ thuật đo lường Vào những năm 20-30 của thế

kỷ 20, để đo các tham số cho những tay máy th hệ đầu thường d ng các chiết áp, biến

áp xoay, Xenxin Khi xuất hiện các Encoder số các thước đo số, người ta cũng đã

áp dụng chúng cho các tay máy nói riêng về cho người máy nói chung Hiện nay để đo các tham số định hướng và các tham số dịch chuyển của các vật thể chuyển động người ta đã bắt đầu ứng dụng các phần tử đo vi cơ quán tính và phi quán tính Ưu điểm

cơ bản của chúng là thước nhỏ, tiêu thụ điện năng nhỏ

Tuy nhiên để áp dụng chúng cho vấn đề đo các tham số của tay máy vẫn là một vấn đề còn mới, đặc biệt là ở Việt Nam Những vấn đề cần giải quyết khi đưa chúng vào ứng dụng cho tay máy hoặc người máy sẽ là một trong những vấn đề cần phải nghiên cứu

Tay máy cũng như người máy là cấu cơ khí có nhiều khớp nối, mà các khớp nối trong điều kiện chế tạo và lắp ráp cơ khí không có độ chính xác cao luôn có độ rơ, gây ra hiện tượng trễ hoặc phi tuyến mềm Đây sẽ là những vấn đề khó khăn khi xây dựng mô hình toán mô t chuyển động của chúng Từ đó cần phải có giải pháp khi thiết kế chế tạo thiết bị điều khiển

Việc điều khiển người máy hoặc tay máy phụ thuộc vào từng nhiệm vụ cụ thể

do người máy hoặc tay máy đảm nhận Các chuyển động của tay máy và người máy thường được mô tả bằng các phương trình phi tuyến hoặc tuyến tính không dừng, vì các tham số của nó thường thay đổi theo thời gian Do vậy khi xây dựng điều khiển tối ưu hoặc thích nghi cần phải có các tiếp cận riêng thuộc vào các bài toán chuyển động cụ thể mà tay máy phải đảm nhiệm

Vấn đề đo lường và điều khiển đã được đặt ra và giải quyết rất nhiều tương ứng với nền tảng của kỹ thuật và công nghệ ứng v i các thời kỳ khác nhau và phụ thuộc vào cơ sở lý thuyết đã được giải quyết trong giai đoạn tương ứng đó

Tại Việt Nam, trong khoảng 20 năm trở lại đây đ có những đề tài, luận án liên quan đến vấn đề chế tạo tay máy, người máy nói chung và vấn đề đo lường điều khiển

Thiện Phúc - Đại học Bách khoa Hà Nộ ề

ứu ứng dụng tay máy -

công nghệ phun phủ bề mặt” (1993), Nghiên cứu ứng dụng robot trong kỹ thuật

hình cơ học, xác định các tham số động lực học của rô b t Sau đó trên cơ sở các

Ba luận án gần đây nhất tại hai trường Đại học của Việt Nam

cụ m – nơ ron và giải thuật di truyền để kh c phục tính bất định của một số tham số

tự hành dạng Nonholonomic và tổng hợp bộ điều khiển bám quỹ đạo

khiển tương ứng với lý thuyết, kỹ thuật công nghệ của những năm tương ứng

,

[19]; mạng nơ ron – c

], [26]; đthích nghi phi tuyến phản hồi đầu ra cho tay máy r t [24]; t

; điều khiển thích nghi trên cơ sở mờ - nơ ron áp dụng

; [17], [21], [23], [25], [30], [31], [34], [37]

Mặc dù đã có nhiều kết quả được công bố,

vẫn còn nhiều vấn đề cần được quan tâm nghiên cứu

giải quyết tiếp nâng cao hơn nữa chất lượng

Chính vì vậy, những nghiên cứu trong lĩnh vực

vẫn luôn cấp thiết và thu hút được sự quan tâm của các nhà khoa học trong và ngoài nước

Sự xuất hiện của công nghệ đo lường vi cơ quán tính và phi quán tính đã được ứng dụng lĩnh vực điều khiển khác nhau như điều khiển vật thể chuyển động hay trong các máy công cụ, các khí t i Tuy nhiên, vấn đề áp dụng cho

vì vậy, khi sử dụng thường phải kết hợp với các phần tử đo phi quán tính khác

sử dụng công cụ bộ lọc Kalman Vấn đề này đã được nhiều tác giả ở Việt Nam và ở các nước khác tập trung nghiên cứu

Để sử dụng được các bộ lọc Kalman phải có thông tin quan sát và phải tính quan sát được Điều này tùy thuộc vào từng đối tượng mà phải có các giải pháp khác nhau để xây dựng bộ lọc

phi quán tính còn ít được đề cập trong các công trình đã công bố Ưu điểm cơ bản của các phần tử đo vi cơ quán tính và phi quán tính là: Gọn, nhẹ, giá thành thấp Tuy nhiên đất nước chúng ta không chủ động chế tạo các thiết bị đo vi cơ này, mà chỉ có thể mua các phần tử đo này tại thị trường

Việc sử dụng các phần tử đo mua sẵn đã phải giải quyết nhiều vấn đề có tính thuật Một trong các vấn đề cần giải quyết đó là:

1 Hiệu chỉnh khi lắp đặt các phần tử đo vào các chi tiết:

Việc lắp đặt các con quay đo tốc độ góc, các gia tốc kế đo gia tốc, các đo véc tơ từ trường cần phải vuông góc với nhau và trong cùng mặt phẳng của hệ tọa độ liên kết Trên hình 1.10 việc nối ghép thực tế các đầu đo trong công nghệ chế tạo thiết bị đo vi cơ quán tính Theo lý thuyết toán học, cần phải đặt 3 đầu đo vào 3 trục

OX, OY, OZ của hệ tọa độ đo OXYZ Song do khi lắp đặt 3 đầu đo nằm ở 3 trục OX’, OY’, OZ’ không trùng với 3 trục lý tưởng OX, OY, OZ Chính vì vậy cần phải

có giải pháp để nhận dạng các sai số lắp đặt để hiệu chỉnh các tham số đo khi tiếp nhận

số liệu đo

Trên hình 1.11 cho thấy: Nếu đầu đo lý tưởng thì tín hiệu đầu ra U tỉ lệ thuận với

chệch không U 0 và hệ số (K+ K) để đưa vào thuật toán số liệu đo

XO

U

k

kk+ k

U0

12

1'

3 Hiệu chỉnh độ lệch hệ quy chuẩn đo so với hệ quy chuẩn lắp đặt

Hệ quy chiếu không gian ba chiều cho các thiết bị đo sau khi được hiệu chỉnh

vậy, khi đưa thông tin vào điều khiển cũng cần phải có thông tin

Hình 1.12 Độ lệch giữa hệ quy chuẩn đo và hệ quy chuẩ

Giải quyết các vấn đề này cho phép tạo ra hệ đo đạt chính xác cao trong điều kiện chế tạo và lắp ráp cơ khí thiếu chính xác trong điều kiện các thước đo được mua ngoài thị trường với các thông số kỹ thuật chưa chính xác

bên

để thiết kế bộ điều khiển hệ thống động học phi tuyến, đó là phương pháp

thiết kế theo dạng cuốn chiếu từ hệ thống con trong cùng (đầu ra) đến hệ thống ngoài cùng (đầu vào)

chúng ta cầ thực hiện song hành cùng việc tính toán bộ quan sát để ước lượng các trạng thái không đo lường được của hệ , các tham số thay đổi của hệ cũng được tính toán, ước lượng bởi các luật cập nhật, đảm bảo tính thích nghi tham số cho hệ có tham số luôn thay đổi

công cụ đo lường mới và công cụ lý thuyết điều khiển để hiện đại hóa hệ

cụm đo và khắc phục các nhược điểm nếu ghép cơ khí, đảm bảo chất lượng điều khiển theo yêu cầu, cụ thể là:

-

Kết luận chương 1:

Từ các phân tích trong chương này cho phép rút ra các kết luận sau:

1 Việc sử dụng tay máy, người máy ngày càng được triển khai rộng rãi trong công nghiệp và đời sống Cùng với sự phát triển của các phần tử đo v lý thuyết điều khiển đòi hỏi phải liên tục giải quyết những vấn đề có tính học thuật khi thiết kế chế tạo

hệ điều khiển cho tay máy, người máy

2 Sự phát triển của phương tiện đo vi cơ quán tính và phi quán tính cho phép tạo

ra các phương tiện đo có tính gọn, nhẹ, giá thành thấp và khắc phục được một số

có tính biến dạng cao (kích thước dài và tải trọng lớn) Tuy nhiên, khi

các tay máy, người máy cần phải giải quyết nhiều vấn đề có tính học thuật

3 Sự xuất hiện giải thuật cuốn chiếu cho phép hoàn thiện các thuật toán điều khiển tay máy, người máy Tuy nhiên đối với từng dạng tay máy cụ thể vẫn còn phải giải quyết những vấn đề có tính học thuật

Chương 2:

XÂY DỰNG THUẬT TOÁN XÁC ĐỊNH TƯ THẾ GÓC

Bộ lọc Kalman là một công cụ toán học giúp cho việc đánh giá véc tơ trạng thái

hệ động học trên cơ sở thông tin quan sát véc tơ tín hiệu đầu ra Giả sử quá trình động của vật thể hoặc quá trình công nghệ được mô tả bởi hệ phương trình động học dưới dạng rời rạc như sau:

X X là trạng thái của véctơ trạng thái X (véc tơ n chi u) ở bước thứ k và (k-1), Z k

là giá trị của véc tơ đầu ra k (véctơ m chi u, thường m n) Véctơ đầu ra

có thể đo được bằng các phương tiện đo F k 1 là véctơ hàm số F ở bước thứ k-1:

T n

f f f

h h h

2.2.1 Ma trận cosin chỉ phương và các góc định hướng

chung (bao gồm chuyển động dịch chuyển của tâm khối và chuyển động quay) luôn có

hai vấn đề, đó là vấn đề định hướng và vấn đề định vị Vấn đề định hướng là xác định

tương quan về góc giữa vật thể và hệ tọa độ chuẩn (hệ tọa độ được lấy làm gốc để

đánh giá sự chuyển động), tức là tương quan giữa hệ tọa độ liên kết và hệ tọa độ chuẩn

[41] Vấn đề định vị là việc xác định vị trí tâm khối và vận tốc của nó so với hệ tọa độ

chuẩn

độ gắn liền với vật thể) được xét trong hệ tọa độ chuẩn A là O X Y Z0 0 0 0

Dịch chuyển tịnh tiến gốc hệ tọa độ chuẩn đến tâm khối vật thể (các trục O X0 0,

O

Z’0

X’0Y’0

O

(a)

2.1 Hệ toạ độ liên kết và hệ toạ độ chuẩn

Thực hiện ba phép quay liên tiếp đ

Các góc mô tả quan hệ giữa hai hệ tọa độ này được gọi là góc định hướng, gồm

Yo

Y

O

1 4

3 2

(d)

- Mặt phẳng nằm ngang; 2 - Mặt phẳng đối xứng; 3 - Mặt phẳng đứng chứa trục dọc; 4 - Mặt phẳng đứng chứa trục OZ

sin sin

(2.6)

sang B và ngược lại, tức là nếu biết véctơ X A (x A y z A A)Ttrong hệ tọa độ A thì véctơ

đó trong hệ tọa độ B sẽ có giá trị X B A X A và ngược lại 1

OZ’0

X’0

Y’0

x y

z

Việc xác định các góc , , hoặc các phần tử aij hoặc cij chính là lời giải của bài toán định hướng Toán học đã chứng minh nếu bằng cách nào đó đo được tốc độ quay của hệ tọa độ B so với hệ tọa độ A (hình 2.3) thì có thể xác định được các tham số định hướng nêu trên [36] Thực tế ngày nay kỹ thuật đã chế tạo được các con quay vi

cơ có kích thước nhỏ cho phép đo các vận tốc góc x, y, z của véctơ quay của hệ tọa độ B so với hệ tọa độ A nếu hệ tọa độ A là hệ tọa độ quán tính [1]

Hiện nay, có hai phương pháp cơ bản xác định ma trận A trên cơ sở thông tin về

tốc độ góc x, y, z Phương pháp dùng 4 số Rodring-Haminton và phương pháp giải

trực tiếp 9 phương trình vi phân mô tả quan hệ các phần tử c ij của ma trận C với các

tốc độ góc x, y, z Quan hệ giữa ma trận C và các tốc độ quay x, y, z theo [41]

được thể hiện như sau:

0 0 0

[36] Tuy nhiên, trong thực tế các con quay (phần tử đo) cho thông tin sau:

0

1 0

2 0

- b 1 , b 2 , b 3 là các tham số biến đổi chậm, thể hiện độ trôi của con quay

- w w w x, y, z là các nhiễu đo, thường có dạng ồn trắng

Như vậy, nếu dùng thông tin của các con quay đưa vào phương trình (2.9) để

giải, chúng ta sẽ nhận được các hàm số c t ij( ) (i = 1, 2, 3, j= 1, 2, 3) không đúng với

sát là các giá trị do từ kế, gia tốc kế và các con quay vi cơ đo tốc độ góc cung cấp để

xác định các hàm c t ij( )của ma trận C

2.2.2 Xây dựng thuật toán xác đị

Đối với thường gặp, việc xác định các phần tử cij của ma trận C

được thực hiện trên cơ sở giải hệ phương trình vi phân (2.10) trên cơ sở thông tin đo

được từ các con quay về tốc độ góc x, y, z [14], [36] Tuy nhiên, kết quả có sai số

phương pháp ước lượng các phần tử cij trên cơ sở dùng bộ lọc Kalman với thông tin

đầu ra từ các từ kế và gia tốc kế gắn trên vật thể B Giả sử trên vật thể B

gắn ba từ kế và ba gia tốc kế theo các trục OX, OY, OZ của hệ tọa độ liên kết (hình 2.4)

4 Sơ đồ bố trí con quay vi cơ đo tốc độ góc, gia tốc kế và từ kế

T x , T y , T z - các từ kế; G x , G y , G z - các gia tốc kế; x, y, z- các con quay vi cơ

Giả thiết trong vùng hoạt động của vật thể, véctơ cường độ từ trường trái đất và

véctơ gia tốc trọng trường có phương và giá trị không đổi Khi hệ tọa độ liên kết trùng

với hệ tọa độ chuẩn đo được các giá trị từ trường B x0, B y0, B z0và gia tốc trọng trường

0 , 0 , 0

g g g Tuy nhiên, khi vật thể B chuyển động thì hệ tọa độ B không trùng với hệ

tọa độ chuẩn A và khi đó các từ kế và các gia tốc kế sẽ cho các chỉ số như sau [15]:

B=C T (B x0 B y0 B z0 ) T , G=C T (g x0 g y0 g z0 ) T (2.12) Đặt:

www

b b b

ễu đo của ba con quay Dùng phương pháp số ơ le giải các phương trình

vi phân (2.14) nhận được 9 phương trình đại số sau:

0 0 2( )k 2(k 1) x 3(k 1) 10(k 1) 3(k 1) z 1(k 1) 12(k 1) 1(k 1) wx 3(k 1) 0 y wz 1(k 1)