NHIỆM VỤ VÀ NỘI DUNG Xây dựng giải thuật quy hoạch quỹ đạo Dubins ngắn nhất qua nhiều điểm Waypoint cho USV; Xây dựng giải thuật né vật cản và làm trơn quỹ đạo Dubins với độ trơn G2; Xâ
GIỚI THIỆU TỔNG QUAN
Lý do chọn đề tài
Phương tiện mặt nước không người lái - USV đã thu hút nhiều nhà nghiên cứu trên thế giới bởi gần đây nhở bởi tính đa dạng trong ứng dụng của nó Nhờ có kích thước nhỏ gọn, tính cơ động cao và khả năng hoạt động trong các môi trường khắc nghiệt mà USV được sử dụng cho hoạt động quân sự , giảm sát môi trường, lập bản đồ dưới nước, vận chuyển hàng hóa, vận chuyển người, cứu nạn cứu hộ [1] [2] [3] Với tính chất địa lí nhiều sông ngòi, ao hồ và giáp biển, Việt Nam là một quốc gia có tiềm năng lớn trong việc ứng dụng USV Song dù đã có xuất hiện những nghiên cứu liên quan tới USV, việc ứng dụng USV vào thực tế còn hạn chế do nhiều trở ngại khác nhau Một trong những trở ngại với USV là sự xuất hiện của các yếu tố không chắc chắn Các yếu tố không chắc chắn này bao gồm sự thay đổi mô hình động học của USV trong quá trình vận hành và sự tác động bên ngoài từ môi trường (sóng, gió, dòng chảy …) Vì vậy tôi chọn đề tài này nhằm phát triển một hệ thống điều khiển USV hoàn chỉnh có thể thích nghi với sự tác động của yếu tố không chắc chắn lên USV.
Mục đích và phạm vi nghiên cứu
Mục đích nghiên cứu của đề tài này là xây dựng giải thuật điều khiển, dẫn hướng và quy hoạch quỹ đạo cho một hệ thống điều khiển USV đáp ứng các yêu cầu sau:
- Quy hoạch được quỹ đạo ngắn nhất đi qua một tập hợp các waypoint và bảo đảm rằng USV có khả năng bám được theo quỹ đạo đã quy hoạch
- Bộ điều khiển có thể thích nghi với các yếu tố không chắc chắn của USV, bao gồm có sự thay đổi về động học hệ thống, động học chưa được mô hình hóa và tác động từ môi trường như sóng, gió,…
Phạm vi nghiên cứu: đề tài này nghiên cứu việc điều khiển USV trong phạm vi môi trường tĩnh có vật cản với các giả định sau đây:
- USV hoạt động trong môi trường nước tĩnh: tác động từ gió, dòng chảy, sóng không quá lớn, các chuyển động roll, pitch, heave có thể bỏ qua
- Các vật cản xuất hiện trong môi trường được biết trước về vị trí, kích thước Các vật cản và waypoint không quá gần nhau Cụ thể là khoảng cách tối thiểu giữa các vùng an toàn của các vật cản với nhau, giữa vùng an toàn của vật cản và waypoint, và giữa điểm waypoint với nhau phải lớn hơn bốn lần bán kính xoay trở của tàu, và đủ để làm trơn quỹ đạo
- Sự xuất hiện của vật cản có thể được bỏ qua khi tối ưu hóa độ dài quỹ đạo
- Số lượng điểm Waypoint được giả định là nhỏ hơn hoặc bằng 20.
Tình hình nghiên cứu
Về giải thuật tối ưu hóa độ dài quỹ đạo: do tính phức tạp của bài toán tối ưu hóa quỹ đạo Dubins (Dubins travelling salesman problem, DTSP) các phương pháp quy hoạch quỹ đạo thường được sử dụng là giải thuật Heuristics và phương án decoupling Tuy nhiên các giải thuật Heuristics thường hoặc không hiệu quả, hoặc đòi hỏi tinh chỉnh tham số hoặc thời gian giải lớn Trong khi đó phương pháp decoupling thường có thể đưa ra lời giải tối ưu hơn, nhưng phụ thuộc vào giả định rằng độ dài quỹ đạo Dubins gần với độ dài đường thẳng, trong khi điều này không đúng với điểm waypoint gần nhau Việc rời rạc hóa góc heading có thể giải quyết vấn đề trên nhưng đổi lại kích thước bài toán trở nên rất lớn Do đó luận văn này cần giải quyết vấn đề cải thiện độ dài quỹ đạo mà không phụ thuộc vào giả định trên mà vẫn nằm trong khả năng tính toán
Về giải thuật làm trơn quỹ đạo: đối với USV, quỹ đạo phải có độ cong không vượt quá giới hạn xoay trở và tối thiểu phải có độ trơn G 2 để USV có khả năng bám hoàn hảo theo quỹ đạo Có nhiều phương pháp để xây dựng quỹ đạo trơn G 2 , như đường
Belzier, B-Spline, … nhưng các phương pháp này rất khó kiểm soát độ cong tối đa Phương pháp dựa trên xoắn ốc Fermat mà Lekkas đề xuất có thể giải quyết vấn đề trên, nhưng lại không có tính nội suy (quỹ đạo không đi qua điểm waypoint) Điều này làm giảm độ chính xác và khiến việc tích hợp giải thuật né bằng quỹ đạo Dubins không thể thực hiện được do quỹ đạo làm trơn đi vào vùng nguy hiểm của vật cản Do đó luận văn này đưa ra cần giải quyết vấn đề làm trơn quỹ đạo với độ trơn G 2 dựa trên xoắn ốc Fermat mà vẫn bảo đảm tính nội suy, và có thể kết hợp với giải thuật né vật cản bằng quỹ đạo Dubins
Về giải thuật dẫn hướng, có nhiều phương pháp được sử dụng để dẫn hướng cho USV, trong đó LOS là phương pháp phổ biến nhất Luận văn này sử dụng phương pháp LOS với khoảng cách nhìn trước Phương pháp này đơn giản, trực quan và đã được chứng minh là có tính ổn định vòng kín trong hệ thống cascade dựa trên giả định về tính ổn định hàm mũ của bộ điều kiến Bên cạnh đó, luận văn giải quyết vấn đề tìm điểm tham chiếu gần nhất nhằm làm đơn giản hóa cấu trúc bộ dẫn hướng
Về giải thuật điều khiển: có nhiều giải thuật điều khiển USV như giải thuật backtracking (BC), giải thuật điều khiển trượt (sliding mode control, SMC), giải thuật dự đoán mô hình (model predictive control, MPC), … Trong đó giải thuật điều khiển trượt thích nghi (ASMC) có thể điều khiển USV ổn định dưới tác động của yếu tố không chắc chắn và có thể sử dụng mô hình động học đã được giản lược hóa, làm giảm độ phức tạp của bộ điều khiển, và hạn chế được hiện tượng chattering do bộ điều khiển SMC gây ra Tuy nhiên giải thuật này có nhược điểm là phụ thuộc vào đạo hàm bậc cao của giá trị heading đặt, dẫn đến hiện tượng xung tín hiệu điều khiển nếu quỹ đạo không phải là đường cong có độ trơn cao Do đó luận văn này đưa ra bộ lọc nhằm hạn chế hiện tượng xung tín hiệu điều khiển.
CƠ SỞ LÝ THUYẾT
Mô hình động lực học USV
Hình 2.1 Hệ thống tọa độ USV Các thành phần tọa độ của USV được mô tả trong Hình 2.1 Quy ước
= là biến vị trí bao gồm vị trí ( x,y ) và góc heading ψ của con tàu trong hệ quy chiếu quán tính Trái Đất và = u v r T là biến vận tốc bao gồm vận tốc dọc
- surge (u), vận tốc ngang - sway (v) và vận tốc góc yaw (r) Dựa theo Fossen và cộng sự [4] , mô hình USV được mô tả bởi những phương trình sau:
Trong đó M là ma trận quán tính, C là thành phần lực Coriolis, D là ma trận lực cản, = x , y , z T là thành phần lực và moment từ động cơ theo trục x y z b , b , b , d là lực nhiễu động Các phần tử của các ma trận trên như sau:
Trong đó: m là khối lượng, x G là vị trí của trọng tâm theo phương dọc, y G giả định bằng 0 do tính đối xứng, các thành phần ,X Y lần lượt là lực theo ba trục x y b , b ,
Trong luận văn này, các giá trị tham số của mô hình Cybership II được sử dụng trong mô phỏng
Cấu trúc hệ thống điều khiển USV
So đồ cho một hệ thống điều khiển USV hoàn chỉnh được trình bày trong Hình 2.2
Hệ thống bao gồm 5 khâu: Mission (Nhiệm vụ), Path Planning (Quy hoạch quỹ đạo), Guidance (Dẫn hướng), Control (Điều khiển), Navigation (Định vị) và USV Dynamics (Động lực học USV) Vai trò của các khâu lần lượt như sau:
- Mission: căn cứ vào nhiệm vụ mà USV cần thực hiện, xác định tập hợp các điểm đến (Waypoints) mà USV cần đi qua và gửi đến khâu Path Planning
- Path Planning: Từ danh sách các Waypoints (WP) được gửi đến và điều kiện ràng buộc, quy hoạch quỹ đạo mong muốn cho USV và gửi đến khâu Guidance
- Guidance: từ vị trí, vận tốc của USV và quỹ đạo mong muốn cho USV, xác định các giá trị đặt về góc và tốc độ cho USV để USV bám theo quỹ đạo
- Navigation: Đo lường và ước lượng các thông tin về trạng thái của USV, bao gồm: vị trí, các giá trị vận tốc và gia tốc
- Control: Xuất tín hiệu điều khiển đến cơ cấu chấp hành để điều khiển USV bám theo tốc độ đặt và góc đặt
- USV Dynamics: Mô hình động lực học của USV
Hình 2.2 Sơ đồ hệ thống điều khiển USV hoàn chỉnh
Đặc điểm và phân loại quỹ đạo
Trong trường hợp khoảng cách di chuyển không đáng kể so với bán kính Trái Đất, quỹ đạo của USV có thể được coi là một đường cong trên mặt phẳng Euclid Gọi C là đường cong với phương trình tham số p t ( ) = x t ( ) ( ) y t T trong đó x t ( ) và y t ( ) là hàm số liên tục trên D Ta có các định nghĩa sau Độ dài đường cong s t ( ) :
= = + (2.11) Độ cong không dấu k t ( ) và độ cong có dấu ( ) t :
Góc tiếp tuyến ( ) t , còn được gọi là góc/hướng hành trình (course angle) đối với quỹ đạo:
Khi chuyển sang hệ tọa độ cực ( ) r , , ta có công thức sau: cos sin x r y r
Độ liên tục hình học (geometric continuity), hay còn gọi là độ trơn (smoothness) của Cđược định nghĩa như sau:
• G 0 : Clà đường cong liền mạch không có chỗ đứt
• G 1 : tại mỗi điểm bất kì trên C xác định được một và chỉ một tiếp tuyến
• G 2 : tại mỗi điểm bất kì trên C xác định được một và chỉ một vector độ cong của đường cong
• G n : tại mỗi điểm bất kì trên C, đạo hàm bậc n của vector của đường cong xác định
GIẢI THUẬT QUY HOẠCH ĐƯỜNG ĐI
Quỹ đạo Dubins
Dubins đã chứng minh rằng với hai WP bất kì và ràng buộc về góc hành trình đầu, góc hành trình cuối và độ cong cực đại thì luôn có ít nhất một quỹ đạo ngắn nhất đi qua các điểm WP đó và thỏa mãn tất cả các ràng buộc trên [5] Quỹ đạo này được gọi là quỹ đạo Dubins Quỹ đạo này có thể gồm 2 đường thẳng 1 cung tròn (CSC) hoặc 3 cung tròn (CCC) Do mỗi cung tròn có hai khả năng rẽ trái hoặc rẽ phải nên tổng cộng có tất cả 6 trường hợp khả dĩ cho quỹ đạo Dubins
Hình 3.1 Các cấu hình quỹ đạo Dubins đi qua hai điểm với heading cho trước
Việc tính toán tham số và độ dài của từng quỹ đạo trong số 6 quỹ đạo trên có thể được xác định một cách hiệu quả dựa theo phương trình được xác lập bởi Shkel và Lumelsky [6], từ đó có thể tìm ra được quỹ đạo ngắn nhất
Theo nguyên lí quy hoạch động, quỹ đạo ngắn nhất đi qua các WP theo một thứ tự cho trước và có ràng buộc về góc hành trình và độ cong cực đại chính là tổ hợp của các quỹ đạo Dubins giữa các WP liên tiếp Do đó, khi thứ tự các WP và góc hành trình đã được xác định, có thể tìm ra quỹ đạo ngắn nhất đi qua các điểm WP trên với độ phức tạp O N ( ) với N là số điểm WP.
Tối ưu hóa đường đi từ quỹ đạo Dubins
Trong một số trường hợp, thứ tự trên quỹ đạo và hướng hành trình tại các WP của quỹ đạo và hướng hành trình tại mỗi điểm không bị ràng buộc cố định, chẳng hạn như trong bài toán khảo sát địa lý Trong những trường hợp trên, bộ quy hoạch quỹ đạo phải giải thêm bài toán là tìm ra tuyến đường tối ưu đi qua các điểm WP sao cho mỗi
WP được đi qua ít nhất một lần Do quỹ đạo Dubins là đường đi ngắn nhất giữa hai
WP, độ dài quỹ đạo giữa các WP hiển nhiên phải tuân theo bất đẳng thức tam giác, do đó mỗi WP chỉ cần đi qua đúng một lần Bài toán tìm đường đi ngắn nhất lúc này trở thành bài toán người bán hàng rong với quỹ đạo Dubins, gọi là Dubins Travelling Saleman Problem – DTSP Bài toán có thể được phát biểu như sau:
Cho tập hợp waypoint S 2 gồm N điểm WP Kí hiệu S n là điểm thứ n của tập
S, n ,1 n N Kí hiệu hoán vị ( ) n là thứ tự của các điểm waypoint S n trên quỹ đạo, và ( ) n 0, 2 ) là góc hành trình của quỹ đạo tại S n Gọi
P n = S n là hàm tọa độ mở rộng của S ( ) n trên quỹ đạo, bao gồm vị trí và góc hành trình, và D là miền của P Như vậy mỗi một quỹ đạo đi qua tất cả các điểm của S và quay về điểm xuất phát ứng với một và chỉ một hàm P Quy ước
P N+ = P Gọi d P x P y ( ( ) ( ) , ) là độ dài quỹ đạo Dubins ngắn nhất đi từ P x ( ) đến P y ( ) và L x y ( ) , = D P x P y ( ( ) ( ) , ) Như vậy đường đi ngắn nhất cần tìm thỏa mãn phương trình sau
Với trường hợp quỹ đạo không quay về điểm xuất phát, ta có thể chuyển về bài toán trên bằng phương pháp tương tự với phương pháp chuyển từ bài toán đường đi
Hamilton sang bài toán TSP Thay tập S bằng tập S = S P , aux với P aux là điểm phụ và thay L bằng Lsao cho L x y ( , ) = L x y ( , ) nếu x y , N + 1 , L ( ) 1, n = , ( 1,1 ) ( , 1 ) 0
L N + =L N N + = và L x y ( , ) = trong các trường hợp còn lại Để có thể làm trơn quỹ đạo, một ràng buộc nữa cần được bổ sung là độ cong quỹ đạo tại mỗi điểm WP cần phải liên tục, điều đó có nghĩa là chiều quay của P n ( ) xác định, do đó ta cần bổ sung thành phần chiều quay ( ) n vào trong tọa độ mở rộng
Lời giải của DTSP có ràng buộc về chiều quay bao gồm 3 yếu tố: góc hành trình
, chiều quay ( ) n và thứ tự WP ( ) n Do không có lời giải tường minh cho góc hành trình ( ) n cho bài toán DTSP và độ dài quỹ đạo ngắn nhất là hàm gián đoạn với số lượng trường hợp rất lớn nên phương pháp chủ yếu được sử dụng là rời rạc hóa góc hành trình Có hai phương pháp chủ yếu để đưa ra lời giải gần tối ưu cho bài toán này: phương án decoupling tách bài toán thành bài toán ETSP cho ( ) n và bài toán
,( ) n , và phương pháp rời rạc hóa thành bài toán GTSP [7]
Phương án decoupling: giả định rằng bán kính quay nhỏ hơn nhiều so với khoảng cách giữa các WP, thì tác động của ( ) n và ( ) n trở nên không đáng kể và có thể xấp xỉ độ dài quỹ đạo Dubins giữa hai điểm bất kì bằng độ dài đoạn thẳng giữa hai điểm đó Lúc này, thứ tự WP * ( ) n có thể được tìm ra bằng cách giải bài toán TSP trên mặt phẳng Euclid (ETSP) Sau khi tìm ra lời giải * ETSP ( ) n , tiếp tục tìm * ( ) n , ( )
( ) n có thể được xấp xỉ bằng cách rời rạc hóa và sử dụng giải thuật Dijikstra Ưu điểm của phương án này là tương đối đơn giản nhưng nhược điểm là lời giải tìm được có thể không tiến tới lời giải tối ưu khi độ phân giải M của ( ) n tiến tới vô cùng do ETSP * ( ) n không phụ thuộc vào M và có thể không trùng khớp với * ( ) n [7]
Với phương án tối giải trực tiếp, đầu tiên miền giá trị của ( ) n được rời rạc hóa thành M giá trị phân bố đều Như vậy mỗi waypoint S n sẽ có 2M tọa độ mở rộng
P n ứng với ( ) n và ( ) n khác nhau Bài toán DTSP khi này trở thành bài toán TSP tổng quát bất đối xứng (Asymmetrical Generalized TSP – AGTSP) trong đó mỗi neighborhood được định nghĩa là N n = P , ( ) n Bài toán AGTSP được biến đổi thành TSP bất đối xứng nhờ biến đổi Noon-Beans [8] Sau khi giải bài toán TSP bất đối xứng, lời giải sẽ được biến đổi ngược trở lại để thu lại lời giải DTSP
Hình 3.2 Sơ đồ giải thuật trực tiếp cho DTSP Ưu điểm của phương án này là khi M tiến đến vô cùng thì lời giải tìm được sẽ tiến đến lời giải tối ưu (giả định lời giải tối ưu không rơi vào trường hợp suy biến) Nhược điểm là kích thước bài toán tăng theo M dẫn đến thời gian giải bài toán tăng lên nhanh
Cụ thể là số đỉnh là NM và số cạnh là NM M ( + 1 ) Để giải quyết vấn đề này, tôi đặt ra hai giá trị phân giải M l ow và M high Trong đó giá trị phân giải thấp M l ow được sử dụng cho phương pháp giải trực tiếp để thu được giá trị * ( ) n , * ( ) n , * ( ) n Sau đó * ( ) n , * ( ) n được tinh chỉnh lại bằng cách tăng giá trị phân giải góc lênM high và sử dụng giải thuật Dijikstra, tương tự như phương pháp 1 Sơ đồ giải thuật được thể hiện trong Hình 3.3
Hình 3.3 Sơ đồ giải thuật với hai mức phân giải Để giải được bài toán theo phương pháp 1 và 2 thì đều cần giải bài toán người bán hàng rong Hiện nay phương pháp tìm lời giải tối ưu nhanh nhất cho bài toán TSP là giải thuật mặt phẳng cắt.
Giải thuật mặt phẳng cắt
Giải thuật mặt phẳng cắt là giải thuật chính xác dành cho bài toán TSP Nguyên lý của giải thuật mặt phẳng cắt là chuyển bài toán đồ thị thành bài toán quy hoạch nguyên tuyến tính (integral linear programming) sau đó thêm vào các điều kiện cần thiết để tìm ra lời giải tối ưu hợp lệ Xét một đồ thị Gbậc n với V i là các đỉnh đồ thị và E ij là của cạnh đi từ V i tới V j Gọi C là một đồ thị con của đồ thị G Định nghĩa các biến x ij như sau:
Tổng độ dài các cạnh thuộc C là
= (3.3) Để C là một chu trình Hamilton thì C phải đáp ứng tối thiểu hai điều kiện Thứ nhất là C gồm n cạnh, và thứ hai là mỗi một đỉnh trong C có một đoạn dẫn tới nó và có một đoạn rời khỏi nó Do đó C thỏa mãn hệ phương trình sau:
Gọi C là đồ thị thỏa mãn hệ phương trình trên sao cho C bé nhất Lúc này ta có
C C Nếu dấu “=” xảy ra, C chính là lời giải cho TSP Trong trường hợp nhỏ hơn, C bao gồm nhiều chu trình Hamilton khác nhau Những chu trình này được gọi là sub-tour Một cách để loại bỏ tất cả các sub-tour là sử dụng phương trình sau:
Hệ phương trình (3.4) và bất phương trình (3.5) được gọi là công thức Dantzig-
loại bỏ các sub-tour tồn tại trong
Qbằng cách giới hạn số cạnh có thể có trong Q nhỏ hơn số lượng cạnh trong cây bao trùm nhỏ nhất của Q Bằng cách áp dụng điều kiện này với mọi đồ thị con với bậc tối thiểu là 2 của G thì sẽ loại bỏ được mọi sub-tour Tuy nhiên số lượng tập con với ít nhất hai phần tử của V là 2 n − n n ( − 1 / 2 ) , điều này có nghĩa là số lượng điều kiện sẽ tăng theo hàm mũ Vì vậy để việc giải bài toán được khả thi thì chỉ các sub-tour nằm trong lời giải hiện tại Sub-tour ra khỏi nghiệm tìm được cho tới khi không còn sub- tour nữa sẽ tìm ra được nghiệm tối ưu Do mỗi bất phương trình thêm vào giống như một phép cắt một phần không gian nghiệm, phương pháp này được gọi là phương pháp mặt phẳng cắt Ngoài các mặt cắt này, phương pháp mặt phẳng cắt còn sử dụng nhiều phương pháp cắt khác nhằm tăng tốc độ tìm nghiệm Tuy nhiên do tính phức tạp của thuật toán, các mặt phẳng cắt trên sẽ không được sử dụng trong luận văn này.
Làm trơn quỹ đạo
Mặc dù quỹ đạo Dubins giải quyết được vấn đề ràng buộc độ cong, song USV chưa thể bám chính xác theo quỹ đạo Dubins Giả sử vận tốc USV luôn khác 0, do gia tốc góc của USV là hữu hạn, do đó đạo hàm độ cong của quỹ đạo của USV cũng là hữu hạn Mặt khác, do độ cong quỹ đạo Dubins chỉ có hai giá trị: 0 và max , vì vậy đạo hàm độ cong của đường cong Dubins không xác định tại các điểm chuyển giữa cung tròn và đường thẳng Vì thế quỹ đạo của USV không thể trùng khớp với quỹ đạo Dubins
Dựa theo định nghĩa đã nêu trong CHƯƠNG 2, độ cong hình học của quỹ đạo Dubins là G 1 , trong khi độ cong của quỹ đạo USV tối thiểu là G 2 Do đó, để USV có thể bám theo quỹ đạo đã quy hoạch, cần phải làm mượt các đoạn cong trên đường cong Dubins bằng đường cong có độ cong từ G 2 Có nhiều loại đường cong G có thể được sử dụng để làm mượt độ cong, ví dụ như B-SPLINE, Bézier, [9] [10] song độ cong lớn nhất của các đường cong này không có dạng tường minh, biến thiên phức tạp và phụ thuộc vào nhiều tham số Lekkas đề xuất hai đường cong là xoắn ốc Fermat và xoắn ốc Clothoid [11] Hai đường cong này đều có độ cong lớn nhất có dạng tường minh và chỉ phụ thuộc vào hai tham số Xoắn ốc Clothoid dễ kiểm soát độ cong hơn xoắn ốc Fermat vì độ cong là hàm tuyến tính theo chiều dài, nhưng tọa độ lại không có biểu thức dạng đóng (closed-form expression), điều này khiến việc tính toán nghiệm khó khăn hơn Vì vậy trong luận văn này, tôi chọn sử dụng xoắn ốc Fermat
Phương trình tham số của xoắn ốc Fermat được cho bởi cos sin x r y r r a
Trong đó r, là tọa độ trục, a0 là hệ số tỉ lệ Độ cong của xoắn ốc Fermat được cho bởi
Từ phương trình (3.7) ta có điểm độ cong cực đại là 7 5
Dựa trên phương trình (3.6) và phương trình (2.16) tiếp tuyến của xoắn ốc Fermat bắt đầu từ 0 được cho bởi:
Cho một xoắn ốc Fermat với 0, f Ta có độ cong cực đại của một cung xoắn ốc là
= trong đó max =min f , m Nếu max có thể được xác định, thì a có thể được xác định bởi:
Với con là độ cong cực đại Đặt FS x ( A , y A , A , fA ) là một được ảnh của phép đẳng cự của xoắn ốc Fermat trên sao cho ảnh bắt đầu từ điểm ( x A , y A ) với góc tiếp tuyến A và kết thúc tại tọa độ góc = fA
Bằng cách kết nối hai xoắn ốc Fermat, ta có thể xây dựng một khớp nối trơn giữa hai đoạn thẳng khác nhau Hai đoạn thẳng tiếp cùng phương với tiếp tuyến tại hai đầu khớp nối và độ cong của khớp nối thay đổi liên tục từ 0 đến max rồi quay về 0 Điều này bảo đảm độ trơn đường cong đạt được độ trơn G2 Đây là phương án mà Lekkas đã sử dụng để làm trơn góc nhọn của quỹ đạo đường thẳng [11] Dựa trên độ thay đổi hướng hành trình (course angle) và điều kiện độ cong cực đại, các tham số và a có thể dễ dàng được tìm ra Nhờ đó quỹ đạo có thể nhanh chóng được xây dựng Tuy nhiên phương án này có vài hạn chế Một hạn chế là quỹ đạo được làm trơn không đi qua các điểm waypoint như quỹ đạo đường thẳng, đây là hạn chế lớn trong trường hợp đòi hỏi tàu phải đi chính xác qua các điểm waypoint, chẳng hạn như trong việc khảo sát địa lý Ngoài ra phương pháp này cũng không thể kết hợp được với phương pháp né vật cản bằng quỹ đạo Dubins vì quỹ đạo làm trơn sẽ trở nên gần vật cản hơn so với quỹ đạo Dubins né vật cản Để giải quyết vấn đề này, tôi sử dụng một phương án khác: thay vì chỉ làm trơn góc, mỗi đoạn thẳng trong quỹ đạo đường thẳng sẽ được thay thế bởi một đường cong chuyển đổi ba đoạn gồm hai xoắn ốc Fermat ở hai đầu và một đoạn tiếp tuyến chung giữa hai xoắn ốc đấy Điều này cho phép đường cong mới đi qua các điểm Waypoint và đồng thời phù hợp để thay thế đường cong Dubins né vật cản Việc xây dựng đòi hỏi đường cong thỏa mãn các điều kiện sau:
1 Hướng hành trình và độ cong tại hai đầu quỹ đạo phải khớp với yêu cầu
2 Đoạn thẳng nối giữa hai xoắn ốc Fermat phải là tiếp tuyến chung giữa hai xoắn ốc tại điểm tiếp xúc
Phần đầu tiên có thể được giải quyết bằng cách xây dựng phương trình tham số xoắn ốc dựa theo 1 Khi 1 và 2 được xác định thì đường xoắn ốc cũng được xác định dựa theo phương trình tham số Từ các phương trình này đồng thời có thể rút ra phương trình góc đoạn thẳng nối giữa hai đầu xoắn ốc và góc tiếp tuyến của hai xoắn ốc theo 1 và 2 , qua đó tìm ra giá trị 1 , 2 thỏa mãn điều kiện thứ hai
3.4.2 Phương trình tham số dành cho đường xoắn ốc Fermat
Gọi FS x y ( 0, 0, , f ) là ảnh đẳng cự của xoắn ốc Fermat với điểm bắt đầu tại
( x y 0, 0 ) với góc tiếp tuyến ban đầu và kết thúc tại = f , đồng thời giả định rằng f m
, điều này cũng có nghĩa là f = max và f = ( ) f =max ( ) FS có phương trình tham số như sau:
Với − 1;1 chỉ định liệu FS có bị lật hay không đồng thời xác định chiều quay cho FS Với độ cong tăng dần từ 0 tới f , xoắn ốc FS tương ứng với đường cong thoát trong đường cong chuyển đổi ba đoạn Tuy nhiên đối với đường cong thoát, chỉ có tọa độ ( x f , y f ) là biết trước, do đó ta cần chuyển phương trình tham số theo
( ) cos cos sin sin f f f FS f f f x a a y a a
p (3.11) Để tạo ra một đường cong vào FS, ta đảo ngược tham số = f −
Các tham số tương ứng là = f , f = , = − Để tiện lợi, ta đặt biến làm biến chỉ định liệu đường cong có bị lật hay không ( =1 nếu bị lật và = −1 nếu không bị lật) Nhờ đó hầu hết các tham số của FS giống với FS, ngoại trừ nhằm cho phép dấu của nó nhất quán với chiều quay của của xoắn ốc Phương trình tham số của xoắn ốc lúc này là:
Các hiệu ứng của và được thể hiện ở Hình 3.4 Chiều và chiều quay của tham số Fermat với các tham số , khác nhau ( =0) Các tham số cho xoắn ốc Fermat trong hình lần lượt là ( x f , y f ) = ( ) 0,0 , a = 2, góc hành trình tại tại ( x f , y f ) là 0 and 8
Hình 3.4 Chiều và chiều quay của tham số Fermat với các tham số , khác nhau ( =0)
3.4.3 Điều kiện độ cong và góc hành trình tại các điểm cuối
Trong mỗi xoắn ốc Fermat của đường cong nội suy ba đoạn, điểm ( x f , y f ) ứng với hai đầu của đường cong nội suy (tức hai vị trí waypoint) Vì vậy, ( x f , y f ) phải thỏa mãn điều kiện sau về độ cong:
Cho trước a, f có thể được suy ra được f và f thông qua phương trình (3.9) Tương tự với f , f bị ràng buộc bởi góc hành trình hai điểm WP Đặt hướng lộ trình là , ta có f = với xoắn ốc thứ nhất và f = + với xoắn ốc thứ hai Thông qua phương trình với sự điều chỉnh của hai biến , ta có phương trình sau arctan 1
Từ đó có thể xác định qua f , , và
3.4.4 Điều kiện tiếp tuyến chung Điều kiện cuối cùng để đường cong đạt được độ cong G 2 là phương của đoạn thẳng nối giữa hai đầu xoắn ốc phải trùng khớp với hướng của đường thẳng nối giữa hai điểm cuối của hai xoắn ốc Fermat như trong Hình 3.5
Hình 3.5 Điều kiện tiếp tuyến chung của đường cong nội suy ba đoạn
Gọi v v 1 , 2 là hai điểm đầu cuối của xoắn ốc Fermat, ta có điều kiện tiếp tuyến chung được phát biểu như sau
Trong đó f f 1 , 2 0, v v 1 , 2 được xác định qua phương trình (3.14) Nghiệm của phương trình (3.19) cũng là điểm cực tiểu của f f 1 , 2 Xấp xỉ Taylor bậc nhất cho ra
1 1 1 f − +O − và f 2 ( − 2 ) 2 + O ( ( − 2 ) 4 ) tại lân cận nghiệm Để thuận lợi cho việc thiết lập điều kiện dừng và tìm nghiệm, ta lựa chọn hàm g 1 = f 1
Nghiệm của hàm số đa biến f không có dạng tường minh Do đó cần xây dựng một phương pháp lặp phù hợp là điều cần thiết để giải phương trình này Luận văn này sử dụng phương pháp gradient descent
Trong đó 1 2 g g T g n là hệ số học, g là ma trận Jacobi Để hạn chế hiện tượng phương pháp lặp không hội tụ do bước đi quá lớn do g lớn, một cận trên được đặt nhằm giới hạn độ lớn của x
(3.22) Để tính được f ta có thể sử dụng quy tắc dây chuyền với g f ( ) , f v χ ( ) , ,
Các thành phần đạo hàm riêng phần trong phương trình (3.23) như sau
( 2 1 1 1 2 1 1 1 ) diag v cos v sin v cos v sin
Các thành phần trong phương trình được tính dựa theo các phần trước đó
GIẢI THUẬT DẪN HƯỚNG VÀ ĐIỀU KHIỂN
LOS với khoảng cách nhìn trước
Có nhiều phương pháp dẫn hướng bám đường cho USV được áp dụng trong thực tế : LOS, Nonlinear Guidance Law (NLGL), Vector Field (VF), … Trong đó, các phương pháp LOS đã được nghiên cứu rất nhiều trong những năm gần đây Trong đó các phương pháp LOS tuy đơn giản nhưng đã được chứng tỏ là hiệu quả trong các thí nghiệm và ứng dụng thực tế Luận văn này sử dụng phương pháp LOS với khoảng cách nhìn trước biến đổi theo sai số cross-tracking do Lekkas và Fossen đề xuất [12] Bên cạnh đó, phương pháp này có thể sử dụng để xây dựng hệ thống dẫn hướng – điều khiển bền vững Phương pháp được thể hiện trực quan trong Hình 4.1
Hình 4.1 Chi tiết phương pháp LOS Look-ahead
Gọi P d =( ) p k là tập hợp các điểm trên quỹ đạo sau khi rời rạc hóa đường đi Giả định 1 p k + 1 −p k 2 Đặt p t * k ( ) là vị trí mong muốn hiện tại của USV trên P d Trong luận văn này p k * được chọn là điểm gần nhất trên quỹ đạo với USV Khi khoảng cách giữa p k * và p k * + 1 đủ nhỏ, góc của tia p p k * k * + 1 (gọi là p ) xấp xỉ bằng hướng hành trình của quỹ đạo tại p * k Sai số along-track x e và cross-track y e được định nghĩa như sau:
( ) sin ( ) cos e d p d p y = − −x x + y−y (4.2) x e là khoảng cách giữa p k và hình chiếu của tọa độ của tàu lên p p * k * k + 1 Do p d được chọn là điểm gần nhất trên quỹ đạo, x e 0 Sai số y e xấp xỉ bằng với khoảng cách nhỏ nhất giữa USV và quỹ đạo mong muốn Như vậy sai số y e được dùng để đánh giá khả năng bám của USV theo quỹ đạo
Một đường chỉ hướng – Line Of Sight (LOS) sẽ được dựng từ trọng tâm của tàu tới điểm P LOS nằm trên tia p p k * k * + 1 Góc của LOS sẽ được dùng làm góc đặt d của USV
Vị trí của P LOS sẽ được xác định bởi khoảng cách nhìn trước (look-ahead distance) giữa P LOS và p k như trong Hình 4.1
Dựa theo Hình 4.1, luật dẫn hướng cho góc Heading được xác định như sau: arctan e d p
Có thể thấy LOS luôn hướng sao cho y e giảm Khi giảm giá trị , luật dẫn hướng sẽ thiên về việc giảm y e , USV sẽ tiến về quỹ đạo nhanh hơn, nhưng có thể dẫn đến dao động do quán tính khi USV bám quanh quỹ đạo Ngược lại, khi tăng giá trị , luật dẫn hướng sẽ giảm dao động, nhưng sẽ khiến USV tiến về quỹ đạo chậm hơn Để cải thiện chất lượng dẫn hướng, Lekkas đề xuất hàm thích nghi theo y e cho giá trị :
Giá trị của sẽ giảm về min khi tàu tiến gần quỹ đạo ( y e →0) và tăng lên đến
max khi tàu ở xa quỹ đạo ( y e → ).
Xác định điểm gần nhất của quỹ đạo so với USV
Với quỹ đạo là đường cong bao lồi (convex hull), điểm tham chiếu gần nhất giữa USV và quỹ đạo có thể được xác định đơn giản bằng cách lấy điểm cực tiểu toàn cục của hàm khoảng cách trên tập hợp P d
Tuy nhiên, với quỹ đạo không phải là đường cong bao lồi, việc xác định điểm tham chiếu gần nhất theo phương trình trên có thể dẫn đến hiện tượng điểm tham chiếu gần nhất “nhảy” trên quỹ đạo như trong hình Hình 4.2:
Hình 4.2 Hiện tượng nhảy điểm gần nhất
Hiện tượng nhảy điểm gần nhất sẽ dẫn đến việc tàu không bám đúng theo quỹ đạo đã hoạch định và bị mất ổn định do sự thay đổi hướng tham chiếu đột ngột Để tránh hiện tượng trên, một phương án có thể sử dụng là xét quỹ đạo liên tục P thay vì quỹ đạo rời rạc P d và chia quỹ đạo thành nhiều đoạn và xác định điểm gần nhất dựa trên phương trình toán học của mỗi đoạn Sự chuyển tiếp từ một đoạn quỹ đạo sang đoạn tiếp theo được xác định dựa trên việc điểm gần nhất có rơi ra khỏi giới hạn của đoạn không, với giả định rằng mỗi đoạn có thể được mở rộng ra khỏi giới hạn đã quy hoạch trước Tuy nhiên, phương án trên đòi hỏi bộ dẫn hướng phải biết về phương trình của quỹ đạo và cấu trúc của bộ dẫn hướng sẽ bị thay đổi theo phương án quy hoạch quỹ đạo được chọn Hơn thế nữa, vị trí của điểm gần nhất của quỹ đạo có thể không phải là hàm số có dạng tường minh theo vị trí của tàu, bao gồm có xoắn ốc Fermat Điều này có nghĩa là bộ dẫn hướng phải sử dụng phương pháp xấp xỉ nghiệm để tìm ra điểm gần nhất, gây khó khăn cho việc đáp ứng theo thời gian thực
Vì vậy, thay vì sử dụng phương án trên, tôi đề xuất phương án sử dụng cửa sổ tìm kiếm để tìm ra điểm gần nhất Cho trước một quỹ đạo rời rạc hóa P d =( ) p k , cửa sổ tìm kiếm được định nghĩa như sau:
Trong đó s là vị trí bắt đầu của cửa sổ trượt, w là độ rộng của sổ trượt Cửa sổ trượt đóng vai trò theo dõi tập hợp các điểm lân cận điểm tham chiếu gần nhất hiện tại Giả định rằng vị trí của điểm tham chiếu gần nhất gần nhất cần tìm thay đổi không quá nhiều, thì vị trí của điểm lân cận gần nhất sẽ nằm trong cửa sổ trượt Như vậy điểm lân cận gần nhất sẽ là một điểm cực tiểu cục bộ nằm trong cửa sổ trượt Ngoài ra, nếu độ lớn của cửa sổ trượt đủ nhỏ, thì cửa sổ trượt sẽ không chứa hai điểm cực tiểu liên tiếp Khi đó điểm tham chiếu gần nhất sẽ chính là điểm cực tiểu toàn cục trên cửa sổ trượt
= − + − (4.7) Độ rộng phù hợp của cửa sổ có thể được xác định như sau max min
Trong đó u max là tốc độ dọc (surge) tối đa, T s là thời gian lấy mẫu, d min là chênh lệnh tối thiểu về độ dài quỹ đạo tại hai điểm cực tiểu liên tiếp Với quỹ đạo Dubins, d min =R, 1 là khoảng cách tối thiểu giữa hai điểm rời rạc trên quỹ đạo và 2 là khoảng cách tối đa giữa hai điểm rời rạc trên quỹ đạo Để duy trì cho điểm p * k nằm trong cửa sổ trượt, vị trí cửa sổ trượt cần được cập nhật sau khi vị trí tàu thay đổi Luật cập nhật được chọn sao cho p * k luôn nằm giữa cửa sổ, tức là *
2 s=k − w , ngoại trừ hai trường hợp biên s=1 và s=N Như vậy thuật toán tìm điểm gần nhất như sau
2 sk − w , lặp lại các bước sau o *
- Kết thúc và trả về p * k
GIẢI THUẬT ĐIỀU KHIỂN
Mô hình không chắc chắn
Mô hình USV được mô tả trong phương trình (2.1) được đơn giản hóa thành mô hình không chắc chắn dựa trên các giả định sau:
1 Giả định A1: USV là một khối rắn, đồng nhất, và có cấu trúc đối xứng
2 Giả định A2: Trọng tâm của USV nằm trên gốc tọa độ của hệ tọa độ cố định theo vật thể
3 Dịch chuyển heave, pitch, roll không đáng kể theo mặt phẳng nằm ngang
4 Các thành phần nằm ngoài đường chéo chính của ma trận quán tính và lực cản tương đối nhỏ so với các thành phần nằm trên đường chéo chính, và có thể bỏ qua
5 Thành phần động lực học không được mô hình hóa, sự nhiễu động tham số, và tác động từ bên ngoài được coi như thành phần không chắc chắn
Từ đó mô hình không chắc chắn của USV có thể được xây dựng như sau [13]:
Các thành phần ( ) u , ( ) v và ( ) r là các thành phần không chắc chắn bị chặn
Thành phần sway không bị tác động trực tiếp bởi tín hiệu điều khiển Vì vậy Y v ( ) và ( )
X u được giả định là bị chặn và Y v ( ) 0
Điều khiển động lực học surge và góc heading
Động học surge được trình bày lại như sau:
Trong đó =u là biến trạng thái và U = x là tín hiệu điều khiển Theo (5.1) ta có
− (5.11) Đặt sai số e u =u d −u với u d là giá trị đặt cho u Định nghĩa mặt trượt như sau u e u u e d u
= + (5.12) Đạo hàm phương trình (5.12), thu được
Kết hợp với phương trình (5.9) ta có:
Do đó ta có luật điều khiển
Trong đó u surge là luật điều khiển mặt trượt thích nghi
Trong đó K u ( ) t là độ lợi thích nghi, với luật cập nhật như sau
K =K (5.18) Độ lợi K u ( ) t được tăng lên khi mặt trượt vượt khỏi ngưỡng cho phép u Khi mặt trượt u nằm trong ngưỡng cho phép, giá trị độ lợi được giảm xuống nhằm làm giảm hiện chattering Cận dưới K min, u 0 nhằm bảo đảm giá trị của u ( ) t luôn dương Tương tự, động lực học heading được trình bày lại như sau
= = r là biến trạng thái, U = x là tín hiệu điều khiển
là thành phần không chắc chắn
− (5.21) Định nghĩa mặt trượt như sau: e e
Với e = d − ,r= Tương tự, d là giá trị đặt Đạo hàm phương trình (5.22) thu được
Kết hợp với phương trình (5.19) ta có
Tương tự với surge, luật điều khiển heading như sau :
Trong đó luật điều khiển mặt trượt u heading có dạng tương tự với luật điều khiển u surge
Sự ổn định vòng kín của bộ điều khiển
Từ phương trình (5.14), (5.15), (5.24), (5.25) và luật điều khiển mặt trượt (5.16) ta có :
= + = − − + (5.27) Động học mặt trượt surge và yaw có chung dạng như sau:
Trong đó là mặt trượt và K a là độ lợi thích nghi Đặt hàm Lyapunov ( ) 1 2 ( )
Trong đó L 1 0 là cận trên chưa biết trước của thành phần không chắc chắn ( ) t Điều kiện ổn định của hệ thống là
Do đó điều kiện của độ lợi thích nghi để là :
Trong trường hợp mặt trượt vượt quá ngưỡng cho phép, , ta có:
Do đó điều kiện đủ để V 0là
Ta có K a ( ) t tăng theo luật cập nhật (5.17) và (5.18), và do 1 1/2 1
L là hằng số, tồn tại t sao cho K a ( ) t thỏa mãn điều kiện (5.33) Hơn nữa, K a ( ) t sẽ tiếp tục tăng tới khi , do đó mặt trượt sẽ quay trở về vùng , như vậy hệ thống ổn định do giá trị mặt trượt luôn quay về vùng giá trị cho phép.
Bộ lọc chống xung
Một vấn đề đối với luật điều khiển góc heading đó là trong luật điều khiển có chứa
+ (phương trình (5.25)) Giá trị của chỉ luôn xác định khi quỹ đạo đã quy hoạch của USV là đường cong G 3 , tuy nhiên đường cong đã được quy hoạch chỉ là đường cong G 2 Điều này có nghĩa là giá trị sẽ lên đến vô cùng tại các điểm nối tiếp giữa các đoạn thẳng và đoạn xoắn ốc của quỹ đạo đã quy hoạch Hơn nữa sai số từ quá trình xấp xỉ nghiệm của tham số quỹ đạo có thể gây ra xung lớn kể cả đối với Vì vậy việc lọc xung cho r d , r d là điều cần thiết Do hiện tượng xung chủ yếu do việc lấy đạo hàm tại các điểm gián đoạn, xung tín hiệu xuất hiện có thể coi như xung delta Dirac ( ) t Các bộ lọc thông thấp hoặc thông dải không hiệu quả với việc lọc xung ra khỏi tín hiệu, vì xung delta Dirac ( ) t có biến đổi Fourier là hằng số trên miền tần số Do vậy tôi đã thiết kế bộ lọc chống xung như sau :
Trong đó u k là tín hiệu cần lọc đã được rời rạc hóa, f k là đầu ra của bộ lọc Khi s 0
− − xấp xỉ bằng đạo hàm du dt , do đó L có thể được coi là giá trị xấp xỉ giới hạn độ lớn cho phép của du dt Khi du L dt , bộ lọc sẽ coi như xung tín hiệu xảy ra, và giữ nguyên giá trị ngõ ra Khi du dt L, bộ lọc sẽ coi như không có xung tín hiệu, hoặc xung tín hiệu đã kết thúc, và cập nhật ngõ ra bằng ngõ vào hiện tại
Do bộ lọc có cấu trúc đơn giản và không có hiện tượng trễ khi không có xung, nhiều bộ lọc có thể được xếp chồng với nhau để tăng hiệu quả lọc xung mà không gây ảnh hưởng đến chất lượng tín hiệu Bên cạnh, một khâu bậc nhất 1
s+ có thể được thêm vô để cải thiện độ trơn của tín hiệu
Tính ổn định của hệ thống sau khi áp dụng bộ lọc chống xung hiện tại chưa được chứng minh Vì vậy trong luận văn này tính ổn định của hệ thống được đánh giá thông qua mô phỏng.
KẾT QUẢ MÔ PHỎNG
Quy hoạch quỹ đạo
6.1.1 Tối ưu hóa đường đi Dubins
Giải thuật quy hoạch tối ưu hóa đường đi Dubins với hai mức phân giải được thực hiện với tập hợp điểm ngẫu nhiên gồm N = 20 điểm được phân bố đều trong vùng
0 x 50, 0 y 50, với bán kính quay R = 2 Mức phân giải thấp M low được chọn để thử nghiệm là M low 4,8,16 và mức phân giải cao là M high dM low Số lượng lần thử của mẫu là 20 lần
Hình 6.1 Ảnh hưởng của việc tăng độ phân giải ( ) n lên quỹ đạo ( N = 10, low 16
Bảng 6.1 Độ dài quỹ đạo (được chuẩn hóa) sau mỗi khâu ở các mức phân giải khác nhau
Số lần thử N M low M high Độ dài sau khi tối ưu hóa ( ) n Độ dài sau khi tối ưu hóa ( ) n và ( ) n
Hình 6.1 minh họa ảnh hưởng của việc tăng độ phân giải lên chất lượng quỹ đạo ( low 16
M = , M high Q2) Độ phân giải thấp dẫn đến trên quỹ đạo xuất hiện nhiều đoạn đi vòng Hiện tượng này là do việc thiếu giá trị góc hành trình đáp ứng được ràng buộc về góc heading và chiều quay quỹ đạo Nhờ vào việc tăng giá trị mà quỹ đạo bớt hiện tượng vòng và giảm được chiều dài, điều này được thể hiện trong Hình 6.1
Bảng 6.1 trình bày giá trị độ dài của quỹ đạo sau khi tối ưu hóa ( ) n và tối ưu hóa
và ( ) n Tại mỗi lần thử, giá trị độ dài được quỹ đạo được chuẩn hóa sao cho độ dài lời giải ứng với mức phân giải thấp nhất (ô in đậm) là 1 Giá trị trong bảng là giá trị trung bình của các lần thử Từ giá trị trong bảng có thể thấy khâu tối ưu hóa góc hành trình ( ) n và chiều quay ( ) n cải thiện rõ rệt độ dài quỹ đạo, và làm giảm sự phụ thuộc về độ dài lời giải so với độ phân giải ban đầu của khâu tối ưu hóa thứ tự
Dựa theo Bảng 6.2 độ phân giải quỹ đạo ban đầu phù hợp có thể chọn là 8
Giải thuật làm trơn quỹ đạo được thực hiện với quỹ đạo đi qua 5 WP, với tọa độ vật cản và tọa độ WP được cho trong Bảng 6.2
Mô phỏng điều khiển tàu được thực hiện trên Simulink với quỹ đạo Dubins chưa làm trơn và quỹ đạo đã được làm trơn bằng xoắn ốc Fermat Bộ điều khiển được sử dụng là bộ điều khiển ASMC được trình bày trong CHƯƠNG 5 Kết quả mô phỏng được thể hiện trong hình và bảng
Bảng 6.2 Tọa độ và bán kính của WP và vật cản
Bảng 6.3 Thông số của mô hình USV (hệ đơn vị kg, m, s)
Tham số Giá trị Tham số Giá trị m 23.8 Y r 0.1079
Hình 6.2 Quỹ đạo Dubins và quỹ đạo mô phỏng của tàu
Hình 6.3 Quỹ đạo Fermat và quỹ đạo mô phỏng của tàu
Hình 6.4 Chất lượng điều khiển với quỹ đạo Dubins
Hình 6.5 Chất lượng điều khiển với quỹ đạo Fermat
Hình 6.6 Độ cong của quỹ đạo Dubins
Hình 6.7 Độ cong của quỹ đạo Fermat
Tiêu chuẩn Quỹ đạo Dubins Quỹ đạo đã làm trơn Sai số cross-track RMS
Sai số cross-track tối đa
Chiều dài đường đi quy hoạch (m) 113.7228 113.8473
Chiều dài đường đi mô phỏng (m) 114.6724 113.9798
Sai số khoảng cách tới các điểm WPi i= [2 3 4 5]
Qua kết quả mô phỏng và bảng số liệu, có thể thấy sai số cross-track đã được giảm một cách đáng kể, với sai số tối đa giảm đi 5 lần và sai số RMS giảm đi 4.5 lần, sai số tại các WP đều giảm đi đáng kể Chất lượng bám của góc heading cũng Mặc dù chiều dài quỹ đạo được làm trơn lớn hơn, nhưng chiều dài USV di chuyển lại được giảm đi so với quỹ đạo Dubins nhờ vào việc chất lượng bám quỹ đạo của USV tốt hơn Bên cạnh đó số lượng xung của tín hiệu điều khiển heading cũng giảm đi đáng kể
Bên cạnh đó vẫn còn xung tín hiệu điều khiển xuất hiện quanh WP thứ nhất Nguyên nhân là do đạo hàm của độ cong quỹ đạo rất lớn (Hình 6.7 Độ cong của quỹ đạo Fermat) mặc dù độ cong quỹ đạo là hàm liên tục Điều này ảnh hưởng đến biên độ mặt trượt và gây ra xung điều khiển.
Giải thuật dẫn hướng và điều khiển
Giải thuật ASMC được so sánh với giải thuật SMC trong điều kiện bị tác động bên ngoài Yếu tố tác động lên USV được sử dụng trong mô phỏng như sau: 2 sin 10 u d = t
10 4 r d = t + Kết quả mô phỏng được trình bày từ trang sau
Hình 6.8 Quỹ đạo của USV với bộ điều khiển SMC
Hình 6.9 Quỹ đạo của USV với bộ điều khiển ASMC
Hình 6.10 Sai số cross-track, góc heading và tín hiệu t z với bộ điều khiển SMC
Hình 6.11 Sai số cross-track, góc heading và tín hiệu t z với bộ điều khiển ASMC
Hình 6.12 Tốc độ surge và tín hiệu điều khiển tốc độ surge khi USV với bộ điều khiển ASMC
Hình 6.13 Tốc độ surge và tín hiệu điều khiển tốc độ surge với bộ điều khiển
Hình 6.14 Độ lợi thích nghi của luật điều khiển SMC
Hình 6.15 Độ lợi thích nghi của luật điều khiển với bộ điều khiển ASMC
Dưới tác động của yếu tố không chắc chắn, bộ điều khiển SMC và ASMC vẫn duy trì được tốc độ và góc heading, và bám sát quỹ đạo với sai số cross-track y e 0.2
Qua hình Hình 6.15 có thể thấy độ lợi thích nghi heading đã giảm được hiện tượng chattering với bộ ASMC so với bộ SMC Xung tín hiệu điều khiển quanh các điểm chuyển tiếp quỹ đạo cũng đã được hạn chế bởi bộ lọc chống xung.