4. Đề xuất cỏc giải phỏp mới, nội dung và phƣơng phỏp nghiờn cứu
3.1.1.Nhận định chung
Từ năm 2009, là thời điểm sau khi ra đời cỏc cảm biến tớch hợp 6-DOF với đầy đủ 3 trục Acc và 3 trục Gyro, một hƣớng nghiờn cứu mới để hiệu chỉnh cỏc MEMS INS đƣợc hỡnh thành. Đó cú số kết quả bƣớc đầu đƣa ra dƣới dạng lý thuyết [91], hay dạng tài liệu trao đổi [88]. Cũng cú một vài cụng trỡnh hoàn thiện ở mức độ đơn giản, nhƣ dự ỏn của Sebastian Madgwick đƣợc bắt đầu từ 2010, hay của Sergiu Baluta trong dự ỏn dự ỏn Starlino Electronics project [87, 88]. Bắt đầu từ thời điểm này, kết quả nghiờn cứu đó đƣợc triển khai và cụng bố rộng rói [92, 93].
Đến cuối năm 2012, với sự ra đời của MEMS INS 9-DOF, phƣơng phỏp tự động điều chỉnh ma trận quay đó cú cơ hội để phỏt triển hoàn thiện. Trƣớc khi đi vào giải phỏp và thiết kế chi tiết, luận ỏn khỏi quỏt lý thuyết cơ bản về sử dụng ma trận quay trong kiểm soỏt và định hƣớng trờn cơ sở cỏc tài liệu trờn và nguyờn lý về tớnh toỏn ma trận. Từ đú ứng dụng cỏc vector từ trƣờng của INS 9-DOF để hoàn thiện phƣơng phỏp tự động hiệu chỉnh ma trận quay và phỏt triển giải phỏp phõn tỏn trong thiết kế hệ thống quản lý giỏm sỏt phƣơng tiện giao thụng vận tải đƣờng bộ, cụ thể là ứng dụng cho xe bus.
Cỏc vector chỉ phƣơng cú thể đƣợc biến đổi giữa cỏc khung tham chiếu xoay với một ma trận 3x3, gọi là ma trận quay R (Rotation matrix). Kết quả chuyển động thể hiện trong khung đối tƣợng tham chiếu b-frame, hay cũn gọi là khung P (Plane), so với khung tham chiếu mặt đất e-frame, hay cũn gọi là khung G (Ground). Cú thể xoay vector bằng cỏch nhõn chỳng với ma trận quay R. Ta quy ƣớc vector Q là vector đại diện, vớ dụ khoảng di chuyển, vận tốc hay gia tốc. Cỏc thành phần của Q đƣợc ký hiệu bằng cỏc chỉ số theo cỏc phƣơng của cỏc trục tọa độ Qx, Qy, Qz. Vector Q ở khung
P (Plane) là QP, ở khung G là QG. Sự liờn hệ giữa hai vector của hai khung tham chiếu này đƣợc tớnh toỏn theo ma trận R nhƣ sau:
[ ] ; [ ] (3.1) QG = RQP
Ma trận cosin chỉ phƣơng DCM chớnh là ma trận quay R (Rotation matrix), đƣợc thể hiện qua cỏc gúc Euler, đƣợc tớnh toỏn nhƣ sau:
[ ] (3.2) (3.3)
Ba cột của ma trận R là kết quả biến đổi của ba vector theo ba trục của đối tƣợng (P) so với khung tham chiếu mặt đất (G). Ba hàng của ma trận R là kết quả biến đổi của ba vector khung tham chiếu mặt đất (G) so với khung tham chiếu đối tƣợng (P). Ma trận R chứa tất cả cỏc thụng tin cần thiết để thể hiện tớnh định hƣớng của đối tƣợng so với với khung tham chiếu trỏi đất. Ma trận R cũng đƣợc gọi là ma trận cosine
chỉ phƣơng, bởi vỡ mỗi phần tử là cosine của cỏc gúc quay cơ sở roll, pitch và yaw. Mặc dự cú thể thấy rằng cú 9 phần tử khỏc nhau trong ma trận R nhƣng thực sự chỉ cú phần tử 3 độc lập, bởi vỡ trong sỏu thành phần gọi là trực giao thỏa món điều kiện ba vector cột vuụng gúc với nhau và độ lớn của mỗi vector cột bằng một. Hai quỏ trỡnh xử lý chống trụi ma trận quay là Quỏ trỡnh xử lý trực giao (Orthogonal) và Quỏ trỡnh chuẩn húa hay bỡnh thƣờng húa (Normalization). Cụng thức (3.1) và cụng thức (3.2) diễn tả làm thế nào để xoay một vector đo đạc trong khung tham chiếu của đối tƣợng (P) vào khung tham chiếu của mặt đất (G). Cụng thức này thể hiện sự chuyển đổi giữa cỏc hệ tọa độ thụng qua ma trận cosines chỉ phƣơng. Cụng thức (3.3) là khai triển của cụng thức (3.1), khai triển cụ thể thụng qua phộp nhõn ma trận.
Ma trận quay R mụ tả sự định hƣớng của một hệ quy chiếu đối với một hệ quy chiếu khỏc. Cỏc cột của ma trận R chứa cỏc vector đơn vị trong một hệ quy chiếu và
cú thể đƣợc ỏnh xạ biến đổi từ cỏc hệ quy chiếu khỏc. Một vector trong một hệ quy chiếu này cú thể đƣợc chuyển vào hệ quy chiếu khỏc bằng cỏch nhõn nú với ma trận quay R. Việc chuyển đổi ngƣợc đƣợc thực hiện bởi cỏc phộp nghịch đảo và chuyển vị ma trận. Vector đơn vị I với chiều dài bằng 1 đƣợc sử dụng trong tớnh toỏn tớch chấm (dot product) và tớch chộo (cross product). Một nhúm quay là nhúm của tất cả cỏc phộp quay cú thể, trong đú bất kỳ hai phộp quay nào trong nhúm cú thể đƣợc kết hợp để tạo ra cỏc phộp quay khỏc, và mỗi phộp quay cú một phộp quay nghịch đảo.
Hỡnh 3. 1: Vớ dụ mụ tả chuyển động của đối tƣợng và đặc điểm của R
Cú thể để mụ tả hoạt động của quỏ trỡnh chuyển động của đối tƣợng so với hệ tọa độ trỏi đất thụng qua một độ dịch chuyển trọng tõm (chuyển động của trung tõm của trọng lực) và xoay (thay đổi hƣớng quanh trọng tõm của lực hấp dẫn). Chuyển động định hƣớng của nú đối với trỏi đất cú thể đƣợc mụ tả bằng cỏch xỏc định một phộp quay quanh cỏc trục. Bằng cỏch bắt đầu với mốc nhất định (vị trớ “chuẩn” đầu tiờn, hay cũn gọi là mốc tham chiếu) với định hƣớng theo một hƣớng xỏc định (xỏc định từ la bàn hoặc GPS), vị trớ của cỏc đối tƣợng sẽ đƣợc sẽ đƣợc xỏc định bằng cỏch ỏp dụng cỏc luõn chuyển liờn tục thụng qua cỏc phộp nhõn ma trận. Bất kỳ vị trớ nào cũng cú thể đƣợc mụ tả nhƣ một phộp quay từ vị trớ "mốc chuẩn" này.
Để mụ tả sự chuyển động của đối tƣợng, ta cần xỏc định trạng thỏi của đối tƣợng so với một hệ quy chiếu phự hợp, thƣờng là hệ tọa độ trỏi đất (e-frame), cũn
đƣợc ký hiệu là hệ tọa độ G (Ground). Định hƣớng của đối tƣợng thƣờng đƣợc mụ tả bằng ba phộp quay liờn tục theo thứ tự theo cỏc trục. Cỏc gúc quay đƣợc gọi là cỏc gúc Euler, bao gồm gúc roll, pitch và yaw. Chuyển động của đối tƣợng thể hiện bởi chuyển động quay của hệ tọa độ vật thể P so với hệ quy chiếu G này. Chuyển động của đối tƣợng đƣợc đƣợc diễn tả bằng phộp quay trong hệ tọa độ thuận tay phải (two right – handed coordinate systems) nhƣ sau [88]:
1. Xoay đối tƣợng đối với trục zb của nú một gúc ψ 2. Xoay đối tƣợng đối với trục yb của nú một gúc θ 3. Xoay đối tƣợng đối với trục xb của nú một gúc
Hỡnh 3. 2: Quay hệ tọa độ vật thể P so với hệ tọa độ trỏi đất G
Trong đú luụn thỏa món cỏc cụng thức sau:
[ ] (3.4) Với: [ ] (3.5)
Lý do mà cỏc nghịch đảo của ma trận xoay R (ký hiệu theo cỏch này là ri,j) bằng chuyển vị của nú là vỡ tớnh đối xứng của cỏc phần tử. Cỏc phần tử của ma trận xoay là cỏc hàm cosine giữa cỏc cặp trục, một trong khung đối tƣợng P và một khung mặt đất G. Tớnh toỏn nghịch đảo là tƣơng đƣơng với việc trao đổi vai trũ của mặt đất
G và khung tham chiếu đối tƣợng P, tƣơng tự việc chuyển vị cỏc hàng và cột.
Quỏ trỡnh chuyển động của đối tƣợng thể hiện tƣơng đƣơng với việc xoay cỏc ma trận liờn tục. Ta cú thể nhõn cỏc quay ma trận với nhau ở mỗi lần xoay và cú đƣợc một ma trận xoay cuối cựng tƣơng đƣơng với việc ỏp dụng tất cả cỏc phộp quay liờn tiếp. Cần chỳ ý tới thứ tự cỏc phộp quay, luụn ỏp dụng cỏc phộp quay liờn tiếp ở phớa bờn trỏi của cỏc kết quả. Lý do mà chỳng ta phải lƣu ý về cỏc chuỗi cỏc hoạt động nhõn khi quay ma trận là phộp nhõn ma trận là khụng giao hoỏn. Do đú thứ tự của cỏc phộp nhõn ma trận là rất quan trọng. Điều này tƣơng đƣơng với việc quay cỏc khung tham chiếu. Vớ dụ, nếu cú ba ma trận luõn chuyển, từ vị trớ A đến B, từ B đến C, và từ C đến D, ta cú thể tớnh toỏn ma trận liờn tiếp sẽ đi từ hƣớng A đến D nhƣ sau:
(3.6)
Trong đú:
RBA = xoay ma trận quay tớnh từ A tới B
RCB = xoay ma trận quay tớnh từ B tới C
RDC = xoay ma trận quay tớnh từ C tới D (3.7)
RDA = xoay ma trận quay tớnh từ A tới D
Hai khỏi niệm đƣợc sử dụng hiệu quả trong tớnh toỏn ma trận quay R và tớnh cỏc phần tử của nú để điều hƣớng là Tớch chấm vector (dot product) và Tớch chộo vector (cross product). Tớch chấm của hai vector A và B, ký hiệu là , là một tớch vụ hƣớng, tớnh bằng cỏch thực hiện một phộp nhõn ma trận, là kết quả của một vector hàng của ma trận A với vector cột của ma trận B. Nú chỉ ra rằng tớch chấm vector tạo ra một kết quả tƣơng đƣơng với cỏc độ lớn của hai vector và cosin của gúc giữa chỳng.
, - [ ] (3.8)
| || | ( ) (3.9) (adsbygoogle = window.adsbygoogle || []).push({});
Tớch chấm cú tớnh chất giao hoỏn: A ⋅ B = B ⋅ A
Tớch chộo của hai vectors A and B, ký hiệu là , là một vector đƣợc tớnh toỏn theo cụng thức:
( )
( ) (3.10)
( )
Tớch chộo vector là vector vuụng gúc với cả hai vector và độ lớn của nú tỷ lệ thuận với độ lớn của cỏc vector và sine của gúc giữa chỳng. Trong tớnh toỏn cập nhật ma trận quay cần xử lý hiệu chỉnh cỏc tham số của cỏc phần tử để đảm bảo tớnh chất vuụng gúc này của cỏc tớch chộo, gọi là xử lý trực giao (Orthogonal).
Với: k là vector đơn vị trực giao của 2 vector u và v (unit vector orthogonal)
Nhƣ vậy ta cú:
( ) ( ) (3.11)
| | | || | ( )
Một trong những đặc tớnh quan trọng của ma trận quay R là tớnh trực giao của cỏc vector thành phần, cú nghĩa là nếu hai vector vuụng gúc trong một khung tham khảo (nhúm quay) thỡ chỳng sẽ vuụng gúc trong mỗi khung tham chiếu. Ngoài ra, độ dài của một vector là khụng đổi trong mọi hệ quy chiếu. Cỏc sai số của INS, chủ yếu là do lỗi số (Numerical errors) làm vi phạm thuộc tớnh này. Vớ dụ cỏc hàng và cột phải đại diện cho vector đơn vị cú cƣờng độ bằng 1, tuy nhiờn do lỗi số cú thể gõy ra làm chỳng sẽ cú giỏ trị nhỏ hơn hoặc lớn hơn. Quỏ trỡnh tớch lũy nếu khụng cú sự điều chỉnh sẽ làm cho chỳng cú thể bị suy biến đến khụng, hoặc tăng rất lớn. Cú thể hỡnh dung là cỏc hàng và cột luụn phải vuụng gúc với nhau, tuy nhiờn lỗi số cú thể làm cho chỳng "nghiờng" với nhau, nhƣ hỡnh dƣới đõy:
Hỡnh 3. 3: Lệch trực giao và tớch chộo
Ma trận quay cú 9 phần tử ,hay cũn gọi là 9 thành phần (element), trong đú trờn thực tế chỉ cú 3 phần tử trong số đú là độc lập. Đặc tớnh trực giao của cỏc ma trận quay về toỏn học cú nghĩa là bất kỳ cỏc cặp cột (hoặc hàng) của ma trận là vuụng gúc, và tổng cỏc bỡnh phƣơng của cỏc phần tử trong mỗi cột (hoặc hàng) bằng 1. Vỡ vậy, cú thể hạn chế 9 yếu tố về 6 yếu tố (x, y, z) thụng qua cỏc phộp biến đổi nhƣ sau:
Nhƣ vậy, một trong những vấn đề cơ bản trong xử lý trụi dữ liệu INS chớnh là đảm bảo tớnh trực giao này. Ngoài sai số trực giao do phộp lấy tớch chộo, trong hệ thống INS số luụn tồn tại những sai số đặc trƣng là cỏc lỗi số (Numerical errors). Cỏc sai số này cũng gúp phần gõy lờn hiện tƣợng lệch trực giao khi tớnh toỏn cỏc thành phần trong ma trận quay, kể cả khi cú cỏc tớn hiệu chớnh xỏc từ cỏc con quay hồi chuyển (Gyro). Ở cỏc hệ thống INS tƣơng tự, cỏc sai số này sẽ phụ thuộc vào vi xử lý đƣợc sử dụng, trong đú tần số lấy mẫu (sample rate) và ADC là những yếu tố chớnh. Cỏc sai số khỏc gõy ra sai số cho quỏ trỡnh xử lý dữ liệu của INS bao gồm:
• Lỗi tớch hợp: Tớch hợp hệ thống số sử dụng một bƣớc thời gian xỏc định và dữ liệu đƣợc lấy mẫu tại cỏc tần số lấy mẫu nhất định khụng phải là ƣớc số của bƣớc thời gian này. Thụng thƣờng cỏc tớnh toỏn bỏ qua cỏc khoảng thời gian lệch khung này. Lỗi này tỷ lệ thuận với gia tốc quay của hệ thống, tức là khi đối tƣợng đƣợc gia tốc quay nhanh trong khoảng thời gian lệch này.
• Lỗi lƣợng tử húa: Khi làm việc với hệ thống số cần quan tõm tới cỏc biến/thanh ghi chứa ghi cỏc giỏ trị. Cỏc phần tử kỹ thuật số này cú độ dài nhất định, vỡ vậy sẽ sinh ra một lỗi lƣợng tử khi chuyển đổi tƣơng tự sang số, và sinh ra bất cứ khi nào thực hiện một phộp tớnh mà khụng bảo vệ tất cả cỏc bit của kết quả. 3.1.2. Nguyờn tắc sử dụng trận quay R trong kiểm soỏt và định hƣớng
(1). Để kiểm soỏt độ dốc (pitch) của đối tƣợng, ta cần biết tƣ thế dốc (pitch attitude) của nú, đại diện bởi phần tử rzx. Bằng cỏch lấy tớch chấm của trục lăn X (roll axis) của nú với mặt đất theo chiều dọc Z (grond vertical), ta tớnh đƣợc một trong những cosine
chỉ phƣơng là rzx. Nú bằng với sine của gúc giữa trục lăn X và mặt phẳng nằm ngang (horizontal plane) trong khung trỏi đất tham chiếu G. Vỡ vậy, phần tử này của ma trận là một dấu hiệu trực tiếp kiểm soỏt độ dốc cho dự trục lăn X của đối tƣợng cú song song với mặt đất hay khụng, và cú thể đƣợc sử dụng trực tiếp trong một vũng phản hồi để kiểm soỏt độ dốc. Khi đối tƣợng nằm ngang khụng dốc, rzx sẽ bằng 0.
(2). Để kiểm soỏt việc xoay (roll) của đối tƣợng, ta cần phải biết tƣ thế bờ ranh giới (bank attitude) của nú, đại diện bởi phần tử rzy. Bằng cỏch lấy tớch chấm trong trục dốc Y (pitch axis) của nú với mặt đất theo chiều dọc Z (grond vertical), ta tớnh đƣợc một
trong những cosines chỉ phƣơng là rzy. Nú bằng với sine của gúc giữa trục dốc Y và mặt phẳng nằm ngang (horizontal plane) trong khung trỏi đất tham chiếu G. Vỡ vậy, phần tử của ma trận là một dấu hiệu trực tiếp kiểm soỏt độ xoay cho dự trục dốc Y của đối tƣợng cú song song với mặt đất hay khụng, và cú thể đƣợc sử dụng trực tiếp trong một vũng phản hồi để kiểm soỏt độ xoay. Khi đối tƣợng nằm ngang khụng xoay, rzy sẽ bằng 0.
(3). Để điều hƣớng (yaw), ta cần biết tƣ thế trệch (yaw attitude) của đối tƣợng với hƣớng chuyển động mong muốn Z. Giỏ trị này tỡm đƣợc bằng cỏch lấy tớch chộo (cross product) của trục lăn X của đối tƣợng với một vector chuyển động theo hƣớng mong muốn. Vector trục lăn X của đối tƣợng là cột đầu tiờn của ma trận R. Trong trƣờng hợp này ta chỉ quan tõm tới cỏc thành phần nằm ngang, vỡ vậy ta thiết lập thành phần Z bằng khụng, cú vector đại diện là [rxx ryx 0]. Ta lấy tớch chộo của vector đú với vector hƣớng mong muốn để cú đƣợc sine của gúc lệch, và lấy tớch chấm để cú đƣợc
cosine của gúc lệch. Điều này đỳng ngay cả khi đối tƣợng xoay lộn ngƣợc (up side down). Để kiểm tra đối tƣợng cú bị xoay theo hƣớng ngƣợc lại so với hƣớng mong muốn (opposite direction) hay khụng, ta lấy tớch chấm (dot product) của trục lăn X với vector hƣớng mong muốn. Nếu nú là giỏ trị õm thỡ đối tƣợng di chuyển lệch hơn 90 độ so với hƣớng mong muốn, tức là đang di chuyển ngƣợc.
(4). Để kiểm tra đối tƣợng cú bị lộn ngƣợc (up side down) hay khụng, ta kiểm tra cỏc dấu hiệu của tớch chấm của trục trệch Z với phƣơng thẳng đứng. Nếu nú nhỏ hơn 0 thỡ đối tƣợng đang bị lộn ngƣợc. Tớch chấm của trục trệch Z (yaw axis) của đối tƣợng với phƣơng thẳng đứng là phần tử ma trận rzz của ma trận xoay R. Khi đối tƣợng đang chuyển động tƣơng đối ngang bằng (khụng dốc), phần tử này cú giỏ trị xấp xỉ bằng 1. Khi đối tƣợng bị lộn ngƣợc, phần tử này cú giỏ trị xấp xỉ khoảng -1. Khi đối tƣợng đƣợc nghiờng đi sang một bờn một gúc 90 độ, phần tử này cú giỏ trị 0. Yếu tố này thƣờng đƣợc ỏp dụng khi kiểm soỏt đối tƣợng bay nhào lộn trong khụng gian.
(5). Để tỡm tốc độ quay của đối tƣợng xung quanh trục trỏi đất thẳng đứng, ta biến đổi vector quay Gyro vào khung trỏi đất tham chiếu G, và lấy tớch chấm với trục thẳng đứng. Điều này tƣơng đƣơng với việc lấy tớch chấm của hàng thứ ba của ma trận R với
cỏc vector quay của con quay hồi chuyển Gyro. Tỷ lệ quay của đối tƣợng xung quanh trục trỏi đất theo chiều dọc bằng .