So sánh phương án lựa chọn sơ đồ bù khử chuyển động kéo theo trong thuật toán định hướng

Kỹ thuật điều khiển & Điện tử H. M. Tưởng, , N. T. Anh, “So sánh phương án lựa chọn thuật toán định hướng.” 16 SO SÁNH PHƯƠNG ÁN LỰA CHỌN SƠ ĐỒ BÙ KHỬ CHUYỂN ĐỘNG KÉO THEO TRONG THUẬT TOÁN ĐỊNH HƯỚNG Hoàng Mạnh Tưởng1*, Lê Tuấn Anh2 , Phạm Trung Dũng1, Nguyễn Tiến Anh3 Tóm tắt: Trong các bài toán định hướng, mối liên hệ động học giữa chuyển động tuyệt đối, chuyển động tương đối và chuyển động kéo theo thường được mô tả bởi các phương trình vi phân. Giải các phương trình vi phân này ta sẽ xác định được giá trị góc hướng của vật mang. Góc hướng tương đối của vật mang có thể được xác định bằng cách bù khử chuyển động kéo theo khi sử dụng một trong hai sơ đồ thuật toán là: sơ đồ có phản hồi ngược hoặc sơ đồ hai nhánh song song. Trong bài báo này chúng tôi sử dụng phần mềm Matlab để mô phỏng quá trình xác định góc hướng vật mang mà việc bù khử chuyển động kéo theo được thực hiện theo hai sơ đồ thuật toán nhằm so sánh ưu, nhược điểm của chúng khi sử dụng trong thuật toán định hướng. Kết quả mô phỏng cho thấy khi sử dụng sơ đồ hai nhánh song song tối ưu hơn vì giảm thiểu số lượng tính toán, đồng thời khi thực hiện việc xác định góc hướng vật mang theo sơ đồ này ta có thêm thông tin về chuyển động góc so với trái đất, cũng như trong không gian tuyệt đối. Từ khóa: Góc hướng, Chuyển động phức hợp, Chuyển động góc, Phương trình động học, Mô phỏng. 1. ĐẶT VẤN ĐỀ Chuyển động quay của một vật mang trong không gian quán tính có thể chia thành hai thành phần là: chuyển động tương đối và chuyển động kéo theo [1]. Chuyển động tương đối là chuyển động của vật mang so với một hệ tọa độ gắn với Trái Đất và được biểu diễn thông qua các giá trị góc hướng. Các giá trị này là những đại lượng cần xác định trong hầu hết các bài toán điều khiển thiết bị bay nhưng lại không đo được trực tiếp. Chuyển động kéo theo là chuyển động sinh ra bởi chuyển động quay quanh trục của Trái Đất với vecto vận tốc góc có biên độ là hằng số, có giá trị rất nhỏ (ωo = 7,2921158553.10 -5 rad/s), và có hướng trùng với hướng trục quay Trái Đất. Để xác định vận tốc góc vật mang trong không gian thường sử dụng các con quay đo vận tốc góc. Do đó, vecto vận tốc góc chuyển động tương đối có thể được xác định thông qua việc bù khử vecto vận tốc góc sinh ra từ chuyển động kéo theo trong kết quả đo đầu ra của con quay. Mối liên hệ giữa chuyển động tuyệt đối, chuyển động tương đối và chuyển động kéo theo được mô tả nhờ các phương trình vi phân biểu diễn mối liên hệ động học giữa các chuyển động này. Giải các phương trình vi phân ta sẽ xác định được giá trị góc hướng của chuyển động tương đối. Trong hầu hết các bài toán điều khiển, giá trị góc hướng của vật mang được xác định thông qua việc tính tích phân số các phương trình động học nhờ máy tính trên khoang trong hệ thống dẫn đường quán tính [4-8]. Thực hiện giải các phương trình vi phân có thể dựa theo một trong hai sơ đồ được mô tả trên hình 1. Trong sơ đồ có phản hồi ngược (hình 1a) thì tín hiệu bù khử được đưa trực tiếp vào bộ cộng trước khi thực hiện việc tính tích phân. Mặt khác, trong sơ đồ hai nhánh song song (hình 1b) thì tín hiệu bù khử chuyển động kéo theo trong thành phần chuyển động tuyệt đối được thực hiện sau khi tính tích phân tín hiệu thu được từ con quay. Về cơ sở nguyên lý, để xác định góc hướng vật mang ta có thể sử dụng một trong hai sơ đồ này. Khi thực hiện tính toán góc hướng vật mang sử dụng các sơ đồ này trong các thuật toán dẫn đường sẽ có các ưu nhược điểm riêng. Tuy nhiên, chưa có một nghiên cứu cụ thể nào đánh giá ưu, nhược điểm của hai sơ đồ này. Trong bài báo này chúng tôi tiến hành mô phỏng trên phần mềm Matlab quá trình xác định hướng vật mang trong không gian để so sánh ưu, nhược điểm trong việc sử dụng hai Nghiên cứu khoa học công nghệ Tạp chí Nghiên cứu KH&CN quân sự, Số 55, 06 - 2018 17 sơ đồ được đưa ra. Kết quả mô phỏng cho thấy khi sử dụng sơ đồ hai nhánh song song tối ưu hơn vì giảm thiểu số lượng tính toán, đồng thời khi thực hiện việc xác định góc hướng vật mang theo sơ đồ này ta có thêm thông tin về chuyển động góc so với trái đất, cũng như trong không gian tuyệt đối. 2. SƠ ĐỒ XÁC ĐỊNH HƯỚNG VẬT MANG KHI BIẾT THÔNG TIN CHUYỂN ĐỘNG KÉO THEO 2.1. Phương trình mô tả chuyển động vật mang Với giả thiết chuyển động kéo theo là chuyển động quay của Trái Đất quanh trục của nó, phương trình chuyển động phức hợp của vật mang có thể được viết dưới dạng quaternion   2 2 2 B B S S r I I aB B B S S r a S aS S I I aS B B B B B B S S                   (1) với В a ─ Vận tốc chuyển động tuyệt đối; В r ─ Vận tốc chuyển động tương đối, SB ─ Quaternion đặc trưng cho chuyển động tương đối; IB ─ Quaternion đặc trưng cho chuyển động trong không gian tuyệt đối; IS ─ Quaternion đặc trương cho vị trí hệ thống dẫn đường quán tính; S S aS  ─ Vận tốc góc tuyệt đối của trái đất trong hệ tọa độ dẫn đường. Việc tích phân các phương trình động học (1) có thể được thực hiện theo một trong hai sơ đồ thuật toán sau: B aB  B rSS BB   2 1   S S aSS BB ~  B r  S aS  S B B aB  S aS  B aВII BB   2 1 S aSII SS   2 1   IIS BS ~ B  S B (а) (b) Hình 1. Sơ đồ thuật toán xác định góc hướng vật mang. Để đưa vào ứng dụng các sơ đồ thuật toán được đưa ra ban đầu ta cần thực hiện đánh giá ưu nhược điểm của nó khi thực hiện tính giá trị góc hướng thiết bị bay. Trên sơ đồ hình 1a vécto vận tốc góc tương đối được xác định bằng hiệu vécto vận tốc góc tuyệt đối (đo được từ thiết bị đo) và vécto vận tốc góc chuyển động kéo theo (là véc tơ vận tốc quay của Trái đất quanh trục của nó). Vecto vận tốc góc tuyệt đối là đại lượng biến đổi nhanh, còn vecto vận tốc chuyển động kéo theo là đại lượng biến đổi chậm. Do đó, việc tích phân được thực hiện với bước tích phân tương ứng với thành phần biến đổi nhanh của chuyển động tuyệt đối. Với sơ đồ hình 1b, góc hướng vật mang được xác định bằng cách bù khử góc sinh ra do chuyển động kéo theo trong thành phần góc chuyển động tuyệt đối. Sơ đồ thực hiện thuật Kỹ thuật điều khiển & Điện tử H. M. Tưởng, , N. T. Anh, “So sánh phương án lựa chọn thuật toán định hướng.” 18 toán này bao gồm hai nhánh song song: Một nhánh thực hiện tính độ dịch chuyển góc chuyển động kéo theo do chuyển động trái đất sinh ra, nhánh còn lại xác định góc sinh ra do chuyển động tuyệt đối của vật mang. Do chuyển động kéo theo là đại lượng biến đổi chậm, còn chuyển động tuyệt đối biến đổi nhanh nên hai nhánh này có thể thực hiện với hai bước tích phân khác nhau tùy theo chuyển động là biến đổi nhanh hay chậm. Từ đó có thể giúp cho việc tính tích phân giảm dung lượng, thời gian trong quá trình tính toán. Ngoài ra, khi sử dụng phương án này ngoài thông tin về vị trí tương đối, ta còn nhận được thông tin về vị trí tuyệt đối của vật mang trong không gian quán tính. 2.2. Mô hình đánh giá sai số thuật toán định hướng Để đánh giá sai số tính tích phân trong quá trình xác định góc hướng vật mang ta có thể sử dụng mô hình mô phỏng như trên hình 2 [2, 3]. Các phương trình chuyển động khung cắc đăng theo thứ tự 3-2-1 Model ВИУС B aˆ B r B  SB MB   ,,    ,, ФФ fix 213 0 QQQBBS s   0 S ω     sin,coscos,cossin 00ω M kω S ω=  0 SB fix Từ bộ phát chuẩn Thuật toán định hướng Sai số góc Sˆ ω Mẫu chuẩn Hình 2. Sơ đồ mô phỏng đánh giá sai số thuật toán định hướng. Trên sơ đồ này, tín hiệu từ bộ phát chuẩn được đưa vào các bộ lọc tạo hình (ΦΦ) dùng để mô phỏng chuyển động tương đối của vật mang là ba góc Ơle ψ, ϴ, γ. Đồng thời, khối tạo vận tốc góc chuyển động Trái Đất (  S ) đưa ra véc tơ vận tốc góc của Trái Đất. Mô hình bộ đo véctơ vận tốc góc (ВИУС) dùng để xác định véctơ vận tốc góc tuyệt đối của vật mang. Khối mẫu chuẩn là phần tử biến đổi các góc định hướng được khởi tạo thành các quaternion (Bs) đặc trưng cho hướng thực của vật mang. Trong khối thuật toán định hướng với những đầu vào là véctơ vận tốc góc tuyệt đối đo được nhờ con quay và véctơ vận tốc góc của Trái Đất, trong đó sử dụng thuật toán bù khử chuyển động kéo theo dựa vào một trong hai sơ đồ đưa ra. Kết quả quá trình xác định hướng vật mang nhờ việc tính Nghiên cứu khoa học công nghệ Tạp chí Nghiên cứu KH&CN quân sự, Số 55, 06 - 2018 19 tích phân theo một trong hai sơ đồ BM được dùng để so sánh với đầu ra từ khối tạo mẫu chuẩn BS để xác định sai số ε. Các bộ lọc tạo hình (ba bộ lọc giống nhau) cho mỗi biến là các bộ lọc bậc hai với mục đích để nhận được ở đầu ra là các giá trị góc quay và đạo hàm của chúng. Các tham số của bộ lọc hai bậc tự do được chọn giống nhau và sử dụng nghiệm giải tích khi có tác động đầu vào. Model (ВИУС) tương ứng với sơ đồ khối ở công trình [2]. Sai số của thiết bị đo không thể hiện trên sơ đồ. Thao tác giải phương trình (1) ban đầu có thể được viết như sau [2, 3]: Đặt ( ) (0) ( ) ( ) (0) ( ) I I B I I S B t B N t S t S N t        với ( )BN t và ( )SN t là các toán tử quaternion thỏa mãn phương trình, khi đó 1 2 1 2 B B B a S S S a N N N N             (2) với (0) 1BN  , (0) 1SN  , ở đây 1 là quaternion đơn vị. Khi rời rạc nó có dạng (với phương pháp tích phân cụ thể và bước tính rời rạc m) 1 0 1 2 1 0 1 2 .... .... I I B I B B Bm m m m I I S I S S Sm m m m B B N B N N N S S N S N N N                    (3) với ,B Sm mN N – Quaternion các nghiệm của phương trình (2), mà ta có thể xem nó là độ dịch tương ứng của các quaternion 1 1 ,I Im mS B  và chúng xác định vị trí hiện thời của hệ tọa độ dẫn đường và vật mang trong không gian tuyệt đối. Quaternion vị trí tương đối SB theo thời gian hiện tại viết dưới dạng truy hồi có dạng 0 10 1 0 110 , , ,S I I S S S B S S S B mm mm B S B B N B N B N B N            (4) Như vậy, vị trí tương đối hiện tại của vật mang – quaternion ,( 1,2,3,...)S mB m  , được tính khi giá trị ban đầu đã biết – quaternion 1S m B  và tính được lượng thay đổi các quaternion ,B Sm mN N . Quá trình tích phân ta sử dụng phương phép tích phân ẩn. Khi xấp xỉ theo đoạn cho vận tốc góc trên mỗi khoảng rời rạc ta có:       1 i i 1 N 1 N 2 4 ti ti c ct i id              i -1N N Kỹ thuật điều khiển & Điện tử H. M. Tưởng, , N. T. Anh, “So sánh phương án lựa chọn thuật toán định hướng.” 20 Suy ra 1 1 i i i i ˆ ˆN 1 1 , N 1 1 4 4 4 4 c ct t i i                                       (5)   1 i ti ti c i d        ─ Vector quay biểu kiến (tín hiệu đầu ra của thiết bị đo vận tốc góc sau mỗi khoảng rời rạc). Tích phân phương trình chuyển động kéo theo được thực hiện với vận tốc biến đổi chậm S S a trong mỗi khoảng rời rạc có thể được thực hiện bằng phương pháp giải tích hoặc phương pháp số. Khi vận tốc chuyển động kéo theo gần như không đổi việc tích phân được tiến hành theo phương pháp giải tích. Khi đó:   31 2 1 S 1 cos , sin , sin , sin , 2 2 2 2 ; | | SS S S S S S I I s s sm m SS S S a aS m m s S aSm m S S t t                       2222 S s S 3 S s S 2 S s S 1 S         sin,sin,sin,cosN mS 3. MÔ PHỎNG VÀ ĐÁNH GIÁ ĐỘ CHÍNH XÁC THUẬT TOÁN ĐỊNH HƯỚNG Quá trình mô phỏng đánh giá độ chính xác thuật toán định hướng trong đó sử dụng hai sơ đồ bù khử chuyển động kéo theo (mô tả trong hình 1) để xác định góc hướng của vật mang được thực hiện theo sơ đồ trên hình 2. Các giá trị góc hướng và đạo hàm của chúng có thể được khởi tạo thông qua việc sử dụng các bộ lọc tạo hình là các bộ lọc bậc hai. Các giá trị khởi tạo góc hướng của vật mang theo thời gian được mô tả trên hình 3. Hình 3. Mô phỏng sự thay đổi các góc hướng  ,  ,  theo thời gian. Trong quá trình mô phỏng, khi sử dụng sơ đồ hình 1a, quá trình tính tích phân được thực hiện với cả hai chuyển động là: chuyển động tuyệt đối đối và chuyển động kéo theo với cùng một bước thời gian Δt. Khi sử dụng sơ đồ hình 1b, quá trình tính tích phân được thực hiện thông qua hai nhánh song song, nên quá trình tính tích phân nhằm xác định góc Nghiên cứu khoa học công nghệ Tạp chí Nghiên cứu KH&CN quân sự, Số 55, 06 - 2018 21 sinh ra do chuyển động kéo theo được thực hiện với hằng số thời gian lớn hơn Δt’. Cụ thể, với tín hiệu khởi tạo được mô tả trên hình 3 thì tần số cao nhất trong dải phổ của chuyển động góc của vật mang là 3Hz. Do đó, khi sử dụng sơ đồ hình 1a thì bước thời gian thực hiện bù khử góc sinh ra do chuyển động kéo theo có giá trị là Δt = 0,025 s. Giá trị này được xác định theo tần số cao nhất trong dải phổ chuyển động góc của vật mang. Trong khi đó, việc bù khử chuyển động kéo theo khi sử dụng sơ đồ hình 1b có thể được thực hiện với bước thời gian lớn hơn là Δt’ = 20Δt, do chuyển động kéo theo là chuyển động của Trái Đất với giá trị vận tốc góc nhỏ hơn rất nhiều so với vận tốc chuyển động tuyệt đối. So sánh kết quả xác định hướng của vật mang khi thuật toán sử dụng một trong hai sơ đồ đưa ra với các góc hướng được khởi tạo  ,  ,  ta có thể đánh giá được sai số khi sử dụng từng sơ đồ. Thông qua kết quả đánh giá sai số ta có thể đưa ra ưu nhược điểm của từng sơ đồ khi sử dụng trong thuật toán xác định hướng. Hình 4 biểu diễn kết quả đánh giá sai số xác định hướng vật mang khi sử dụng hai sơ đồ đưa ra. Hình 4. Sai số xác định hướng của vật mang 1 - Khi dùng sơ đồ hình 1b; 2 - Khi dùng sơ đồ hình 1a. Kết quả mô phỏng trên hình 4 cho thấy sai lệch trong việc xác định góc hướng của vật mang khi sử dụng hai sơ đồ là rất nhỏ, không đáng kể, cỡ 10-5 rad. Do đó, sai số xác định hướng vật mang khi sử dụng hai sơ đồ tương đương nhau. Tuy nhiên, khi sử dụng sơ đồ hình 1b, số lượng phép tính cần thiết cho thuật toán định hướng ít hơn do bước thời gian thực hiện việc bù khử lớn hơn (Δt’ > Δt). Ngoài ra, khi sử dụng sơ đồ này ta có thêm thông tin về góc gây ra do chuyển động kéo theo và chuyển động trong không gian tuyệt đối. Do vậy, sơ đồ hình 1b thường được sử dụng trong các thuật toán để thực hiện quá trình tính toán các góc hướng vật mang. 4. KẾT LUẬN Bài báo đã đề xuất phương án lựa chọn sơ đồ xác định hướng vật mang trong thuật toán định hướng khi biết thông tin chuyển động kéo theo của vật mang như sau: - Nghiên cứu hai sơ đồ thuật toán mô tả phương trình động học của chuyển động phức hợp gồm: sơ đồ có phản hồi ngược và sơ đồ hai nhánh song song. - Tiến hành khảo sát, mô phỏng sử dụng phần mềm Matlab hai sơ đồ nhằm đưa ra những ưu điểm và nhược điểm của hai sơ đồ khi sử dụng để giải phương trình chuyển động phức hợp của vật mang. - Việc tích phân theo sơ đồ hai nhánh song song cho phép ta sử dụng các bước thời gian khác nhau, với chuyển động chậm ta sử dụng bước tích phân lớn hơn nên dung lượng tính toán ta cần ít hơn. Kỹ thuật điều khiển & Điện tử H. M. Tưởng, , N. T. Anh, “So sánh phương án lựa chọn thuật toán định hướng.” 22 TÀI LIỆU THAM KHẢO [1]. Vũ Duy Cường. “Giáo trình cơ lý thuyết”. NXB Đại Học Quốc Gia 2002, trang 227. [2]. Лобусов .Е.С., Фомичев А.В, “Формирование алгоритмов бесплатформенной инерциальной системы навигации и основных режимов функционирования системы управления малогабаритного космического аппарата. Часть 1”//Мехатроника, автоматизация, управление. – 2014. – Т.16, № 12. – С.60-65. [3]. Лобусов .Е.С., Фомичев А.В. “Алгоритмизация основных режимов функционирования бесплатформенной инерциальной системы навигации и управления движением малогабаритногокосмического аппарата” //Мехатроника, автоматизация, управление. – 2015. – Т.16, № 1. – С.54-59. [4]. Salychev O.S. “Applied Inertial Navigation: Problems and Solutions”. — Moscow: BMSTU Press, 2004. [5]. Salychev O.S. “MEMS-based Inertial Navigation: Expectations and Reality”. — Moscow: BMSTU Press, 2012. [6]. Salychev O.S. “Inertial Systems in Navigation and Geophysics”. — Moscow: BMSTU Press, 1998. [7]. Titterton D.H. , Weston J.L. “Strapdown Inertial Navigation Technology 2nd”. Ed. The Institution of Electrical Engineers, 2004.P.492-493. [8]. Veremeenko, Savelev V.M. “In-flight alignment of a strapdown inertial navigation system of an unmanned Aerial vehicle”. Journal of computer and systems sciences international, 2013, Vol. 52, №.1. pp.106-116. ABSTRACT OPTIMIZATION ALGORITHMS FOR THE PROBLEM OF ORIENTATION SYSTEM OF CARRIER VEHICLES WHEN KNOWING THEIR INFORMATION In the oriental problems, absolute motion, relative motion, and tracking motion have a kinetic relationship that is generally described by differential equations. By solving these equations, the angular value of the carrier is determined. In general, to solve the kinetic differential equations of angular motion of a carrier, two diagram methods are applied namely: a reverse-feedback diagram or a parallel- branch diagram. In this paper, these two diagrams are employed to solve the complex motion of a carrier and the Matlab software is used to simulate their computational processes. The simulated results will identify the advantages and disadvantages of each diagram. We found that the parallel-branch diagram is more optimal due to its less calculated operations. Furthermore, using the parallel- branch approach leads to other benefits such as more information about the angular motion of the carrier to the earth as well as its position in absolute space will be obtained. Keywords: Directional angle, Absolute motion, Relative motion, Dynamic equation, Simulation. Nhận bài ngày 11 tháng 5 năm 2018 Hoàn thiện ngày 04 tháng 6 năm 2018 Chấp nhận đăng ngày 08 tháng 6 năm 2018 Địa chỉ: 1Khoa Kỹ thuật Điều khiển, Học viện Kỹ thuật quân sự; 2Viện Tên lửa, Viện KH & CN quân sự; 3Khoa Hóa - Lý Kỹ thuật, Học viện Kỹ thuật quân sự. *Email: manhtuongbm@yahoo.com.