3 HỆ THỐNG TRÒNH VA CHẠM
3.3 Chế độ hoạch định chủ động
3.3.1 Quỹ đạo tránh va chạm
Như đã phân tích ở mục 3.2, vùng va chạm dự đốn giữa OS có quỹ đạo như (3.1) và TS có quỹ đạo như (3.2) là một threat ellipse tĩnh được mơ tả bởi phương trình (3.8). Để ý thấy rằng trong phương trình (3.1) nếu ta thay đổi
yp(t) và giữ nguyên xp(t) thì các tham số của threat ellipse vẫn không thay đổi. Ta sẽ áp dụng tính chất này để biến đổi bài tốn tránh va chạm với một vật cản động thành một bài toán tránh một vùng va chạm tĩnh là TE. Để giải quyết bài toán mới này, luận văn đề xuất một quỹ đạo an toàn, khả thi và tối ưu tên là quỹ đạo liên tục độ cong theo ê-lip (Ellipse-based Curvature Continuous Trajectory - ECCT). Về tính an tồn, ECCT được hoạch định khơng cắt TE, tức là mọi điểm thuộc ECCT đều khơng thuộc vùng va chạm, do đó OS sẽ được đảm bảo an tồn khi bám theo ECCT. Về tính khả thi, ECCT là đường cong liên tục đến đạo hàm bậc 2 và độ cong quỹ đạo có được điều chỉnh sao cho phù hợp với khả năng xoay sở của OS. Cuối cùng, việc ECCT được xây dựng dựa trên threat ellipse sẽ giúp tối ưu quỹ đạo đi so với các phương pháp khác, điểm này sẽ được làm rõ hơn trong phân tích và so sánh kết quả mơ phỏng. Phương trình của ECCT sẽ được định nghĩa như sau:
xECCT(t) =xu0+vxt
yECCT(t) =fECCT (ξL, ξR, θL, θR, κmax, xECCT)
,∀t ∈[ts, tf] (3.10)
trong κmax là giới hạn xoay trở của OS, ξL, ξR, θL, θR là các tham số của ECCT,
fECCT là phương pháp xây dựng ECCT bằng kết hợp những quỹ đạo mẫu với vai trị khác nhau. Có 5 loại quỹ đạo mẫu tạo nên ECCT gồm:
• Quỹ đạo rẽ trái FL
χ, với χ là góc rẽ. • Quỹ đạo rẽ phải FχR.
Luận Văn Thạc Sĩ 29 CHƯƠNG 3. HỆ THỐNG TRỊNH VA CHẠM
Hình 3.3: Minh họa ECCT và các quỹ đạo mẫu.
• Đường cong ellipse Eθ1,θ2.
• Quỹ đạo phản chiếu qua gương của FL χ, FR
χ, ký hiệu là FLI χ , FRI
χ .
Hình 3.3 biểu diễn ECCT được xây dựng từ các đoạn quỹ đạo hoạch định theo bộ thông số ξL, ξR, θL, θR và được ghép nối thứ tự như sau: FξL/2L , FξL/2LI , S, FζLR,
EθL,θR, FR
ζR, S, FL
ξR/2, FLI
ξR/2 . Quy trình chi tiết hoạch định các đoạn quỹ đạo này sẽ được trình bày chi tiết dưới đây.
Bước 1: EθL,θR
EθL,θR sẽ được trích xuất từ phương trình threat ellipse (3.9) trong miền [θL, θR]. Tuy nhiên, giới hạn xoay trở của OS đôi khi nhỏ hơn độ cong củaEθL,θR, độ dài các trục của TE nên được điều chỉnh để phù hợp với khả năng xoay trở của OS trong những trường hợp này. Gọi κEM là độ cong của EθL,θR, ˘a,˘b là độ dài hai
trục mới của threat ellipse thỏa mãn phương trình sau: ˘ a = ˘b= 1
κmax Nếu κmax < κEM, a < κmax1 , b < κmax1
˘
ab
(a˘2cos2θem+b2sin2
θem=κmax)3/2,˘b=b Nếu κmax < κEM, a < 1
κmax < b
˘
a =a, a˘b
(a2cos2θem+˘b2sin2
θem=κmax)3/2 Nếu κmax < κEM, b < κmax1 < a
(3.11)
Bước 2: FζLR
FζLR được thiết kế theo mơ hình đường xoắn ốc Fermat [30] với độ cong bắt đầu
từ 0 và thay đổi đến một giá trị tùy ý tại điểm kết thúc. Phương trình tốn của đường xoắn ốc Fermat được biểu diễn như sau:
pF S = xF S(θ) yF S(θ) = x0+k√ θcos(ρθ+χ0) y0+k√ θsin(ρθ+χ0) (3.12)
trong đó θ ∈[0, θe], k là hệ số tỉ lệ, [x0, y0] là điểm bắt đầu, χ0 là góc tiếp tuyến tại [x0, y0], và ρ = 1 nếu xoay ngược chiều kim đồng và ρ = −1 nếu ngược lại. Đầu tiên, θe được xác định theo các phương trình sau [30]:
ζL =χe−χ0 (3.13)
θe+ arctan(2θe) =ζL (3.14)
với χ0 và χe được xác định từ ràng buộc liên tục bậc 1 tại các điểm nối quỹ đạo
S−FζLR và FζLR −EθL,θR: χ0 = ξL (3.15) χe= arctan 2( ˙yE(θL),x˙E(θL)) (3.16) Tiếp theo, ta có: k = 2 √ θe3 + 4θ2 e κF S(θe) (1 + 4θ2 e)(3/2) (3.17)
với κF S(θe) là độ cong của pF S tại θe, mà κF S(θe) có thể được tính được nhờ ràng buộc liên tục bậc 2 tại điểm nối FR
ζL−EθL,θR như sau: κF S(θe) = x˙E(θL)ăyE(θL)−xăE(θL) ˙yE(θL) ( ˙x2 E(θL) + ˙y2 E(θL))3/2 = ab a2sin2(θL) +b2cos2(θL) (3.18) Cuối cùng, ta thayθe,χo, k vào (3.12) kết hợp ràng buộc liên tục bậc 0 tại điểm nối FζLR −EθL,θR , ta thu được điểm bắt đầu [x0, y0] như sau:
x0 y0 = xE(θL) yE(θL) − k√ θecos(ρθe+χ0) k√ θesin(ρθe+χ0) (3.19) Bước 3: Đoạn thẳng S
Luận Văn Thạc Sĩ 31 CHƯƠNG 3. HỆ THỐNG TRÒNH VA CHẠM
định ở Bước 4. Bên cạnh đó, S phải được thiết kế sao cho thỏa mãn ràng buộc liên tục quỹ đạo đến bậc 2 tại điểm nối S−FR
ζL, nên đoạn thẳng S phải đi qua điểm [x0, y0] của FR
ζL và có độ dốc là ξL.
Bước 4: FL
ξL/2, FLI ξL/2
Nhưng đoạn cong này giúp OS chuyển tiếp từ bám đường toàn cục sang bám ECCT và ngược lại một cách trơn tru. FL
ξL/2, FLI
ξL/2 được tính tốn bằng cách sử dụng kĩ thuật được đề xuất bởi [31] để tạo một đoạn cong kết nối đường đi toàn cục và đoạn thẳng S của ECCT.
Bước 5: FζRR, S, FξR/2L , FξR/2LI ở nửa bên phải.
Quy trình trình tính tốn tương tự với Bước 2, 3, 4 nhưng các tham số ξL, ζL
được thay thế bằng ξR, ζR.
3.3.2 Tối ưu hóa các tham số của ECCT
Trong mục trước 3.3.1 đã được trình bày quy trình xây dựng quỹ đạo tránh va chạm ECCT từ một bộ thơng số cho trước. Mục này sẽ trình bày phương pháp xác định bộ thơng số này bằng cách giải bài toán tối ưu đa mục tiêu với chỉ tiêu chất lượng được sử dụng để đánh giá bộ thông số gồm: (i) Tổng độ dài quỹ đạo (3.20), (ii) Tổng sai lệch so với đường toàn cục (3.21).
T L(.) =Z tf ts q ˙ x2ECCT + ˙yECCT2 dt (3.20) Sloss(.) =Z xf
xs dev(xECCT, yECCT) dt (3.21) trong đó xs = xECCT(ts), xf = xECCT(tf), dev(xECCT, yECCT) là khoảng cách từ điểm (xECCT, yECCT) đến đường toàn cục.
Như vậy bài toán tối ưu tham số sẽ được định nghĩa như sau:
ξL∗, ξR∗, θL∗, θ∗R = arg min
ξL,ξR,θL,θRT L(xECCT, yECCT) +γSloss(xECCT, yECCT)
s.t. yECCT =fECCT (ξL, ξR, θL, θR, κmax, xECCT) (3.22) Do việc tính đạo hàm của hàm chi phí trong trường hợp này gần như là không thể nên các phương pháp tối ưu hóa liên tục như thuật tốn suy giảm độ dốc
sẽ khó mà áp dụng để giải bài tốn (3.22), thay vào đó các phương pháp tối ưu thông minh dựa trên chiến thuật bầy đàn (Particles Swarm Optimization, Ant Colony), hay sự chọn lọc tự nhiên (Genetic Algorithm, Evolution Algorithm) là các lựa chọn tốt. Tuy nhiên, bài toán tối ưu hóa lại khơng phải mục tiêu chính của luận văn cho nên việc tối ưu (3.22) có thể đơn giản được giải quyết bằng hàm f mincon() được cung cấp bởi Matlab.