Chương 2. Nghiên cứu xây dựng thuật toán
2.2 Xây dựng thuật toán
2.2.3 Dịch chuyển hệ trục tọa độ
Tọa độ của những điểm thuộc biên dạng VL xác định từ bước trên là tọa độ ứng với hệ trục tọa độ của laser, gọi là /base_laser. Trên thực tế, vị trí và hướng của laser gắn trên robot thường sẽ không trùng với vị trí và hướng tại tâm quay robot (/base_link), do vậy ta cần phải sử dụng phép nghịch đảo để dịch chuyển tọa độ từ hệ trục /base_laser sang /base_link. Thông thường, cảm biến gắn trên robot thường sử dụng 3 phép dịch chuyển: tịnh tiến theo trục x và y; xoay góc theo trục z và nghịch đảo theo trục x.
- Nghịch đảo: Cảm biến laser thường được gắn trên robot theo chiều thuận hoặc được lộn ngược lại xoay tại trục x góc 180 độ. Trong bài báo này ta sử dụng laser 2D nên giá trị trên trục z có thể được bỏ qua. Vì vậy, khi xác định được hướng gắn của cảm biến, nếu cảm biến hướng lên trên thì tọa độ của những điểm xét được giữ nguyên, nếu cảm biến hướng xuống dưới thì ta biến đổi theo công thức:
(10) Trong đó: (x, y) là tọa độ của điểm xét trên hệ trục trước khi nghịch đảo, (x’, y’) là tọa độ của điểm xét trên hệ trục sau khi nghịch đảo.
- Xoay góc: Hướng trục x của cảm biến xét tại tâm quay trục z có thể trùng hoặc lệch so với hướng của robot, vì vậy ta cần nghịch đảo góc quay của những điểm xét để tương ứng với tọa độ xét theo hướng của robot. Ta xét các trường hợp có thể xảy ra khi nghịch đảo góc xoay.
Một số định nghĩa:
- XOY là hệ trục tọa độ trước khi xoay.
- X’OY’là hệ trục tọa độ sau khi xoay.
- T là điểm xét (tùy vị trí của T mà áp dụng công thức khác nhau).
- TQ, TH là tọa độ (x, y) của điểm T trên hệ trục tọa độ XOY.
- TQ’, TH’là tọa độ (x’, y’) cần xác định của điểm T trên hệ trục tọa độ X’OY’. -θ là góc lệch giữa hai hệ trục tọa độ (góc lệch giữa hướng lắp của laser so với
hướng của robot).
- là góc của tam giác THO, được xác định bởi công thức:α 𝑡𝑎𝑛(α) = |𝑇𝐻𝑇𝑄 (11)
| |
|
- là góc của tam giác THβ ’O hoặc TQ’O, được xác định thông qua hai góc và .θ α - TO là độ dài đoạn thẳng l tạo bởi hai điểm T và O, được xác định:
(12) 𝑙 = (𝑥
2 − 𝑥
1)2 + (𝑦
2− 𝑦
1)2
Hình 2.8: Một số định nghĩa được sử dụng
Chúng ta cần xác định tọa độ của điểm T bất kỳ trên hệ trục tọa độ X’OY’ khi biết tọa độ của điểm đó trên hệ trục tọa độ XOY. Do điểm T bất kỳ nên ta phân ra các trường hợp khác nhau để xác định tọa độ.
Bản chất của việc xoay góc là đưa ra các phép tính toán để xác định một điểm thuộc hệ trục tọa độ này có giá trị tọa độ nào trong hệ trục tọa độ đích. Xét trường hợp tổng quát, phép xoay hệ trục tọa độ tại tâm O giữa hệ trục tọa độ XOY và X’OY’sẽ tạo ra tám trường hợp, mà tại đó, giá trị của điểm xét sẽ phù hợp với những phép toán khác nhau. Các trường hợp và phép toán tương ứng sẽ được phân loại trong hình bên dưới:
Hình 2.9: Các trường hợp có thể xảy ra khi thực hiện phép xoay góc
- Tịnh tiến: Sau khi tiến hành nghịch đảo phép quay tại trục z, ta sẽ có được tọa độ điểm T trên hệ trục tọa độ song song với hệ trục của robot. Tuy nhiên, tọa độ này cần được dịch chuyển tịnh tiến theo trục x và y để chuyển sang hệ tọa độ của robot.
Một số định nghĩa:
- X’OY’là hệ trục tọa độ trước khi tịnh tiến (sau khi dùng phép quay nghịch đảo).
- X’’O’’Y’’là hệ trục tọa độ sau khi tịnh tiến (hệ trục tọa độ của robot).
- T (x’, y’) là tọa độ điểm T trên hệ trục tọa độ X’OY’.
-∆𝑥 ∆𝑦, là khoảng cách lắp đặt giữa vị trí của cảm biến laser và tâm quay của robot, lần lượt theo trục x và trục y.
- T (x’’, y’’) là tọa độ điểm T trên hệ trục tọa độ X’’O’’Y’’, lần lượt theo trục x và y.
Hình 2.10: Dịch chuyển tịnh tiến
Chúng ta cần xác định tọa độ của điểm T bất kỳ trên hệ trục tọa độ X’’O’’Y’’khi biết tọa độ của điểm đó trên hệ trục tọa độ X’OY’. Do điểm T bất kỳ nên ta phân ra các trường hợp khác nhau để xác định tọa độ. Trong thí nghiệm này, ta cố định cảm biến laser tại góc phần tư thứ hai của hệ trục tọa độ robot, vì vậy ta chỉ xét những trường hợp tại góc phần tư này (những trường hợp khác phân tích tương tự).
(13)
2.2.4 Xác định vị trí của robot
Ta áp dụng phương pháp dịch chuyển hệ trục tọa độ cho những điểm thuộc đoạn VL (P1, P2, P3, P4) đã được xác định từ bước trên. Xây dựng phương trình đường thẳng L đi qua 2 điểm P3 và P4.
Hình 2.11: Sai số vị trí và hướng của robot so với hệ trục tọa độ của VL dock Gọi hệ trục tọa độ XOY là gốc và hướng của trạm sạc, hệ trục tọa độ X’O’Y’là gốc và hướng của cảm biến laser, hệ trục tọa độ X”O”Y”là gốc và hướng của tâm robot.
Khoảng cách OK chính là vị trí của robot so với trạm sạc theo trục x, khoảng cách O’’K là vị trí của robot so với trạm sạc theo trục y và gócϕlà góc lệch giữa hướng của robot và trục x của trạm sạc.
- Sai số vị trí
Xác định khoảng cách x từ tâm O” đến đường thẳng L. Khoảng cách -x chính là tọa độ của tâm robot O”tới hệ trục tọa độ XOY theo trục X.
(14) 𝑑 = |𝑎𝑥0+𝑏𝑦0+𝑐|
𝑎2+𝑏2
Trong đó: -𝑎𝑥 + 𝑏𝑦 + 𝑐 = 0là phương trình của đường thẳng L.
- O”(x0; y0) là tọa độ điểm O”trên hệ trục tọa độ X”O”Y”.
Xây dựng phương trình đường thẳng L’vuông góc với đoạn thẳng L và đi qua điểm P4. L’ vuông góc với L nên phương trình đường thẳng của L' sẽ nhận (-b, a) là vectơ pháp tuyến. L' đi qua điểm P(x4; y4) nên ta có 𝑐 = 𝑏𝑥 . Sau đó xác định
4− 𝑎𝑦
4
khoảng cách y từ tâm O’’đến đường thẳng L’.
Xác định khoảng cách P1P4 theo công thức tính độ dài đoạn thẳng đi qua hai điểm.
(15) 𝑂"𝐾 = 𝑦 − 𝐾𝑍 = 𝑦 − 𝑃12𝑃4
Ta thấy, O’’K chính là tọa độ của tâm robot O”tới hệ trục tọa độ XOY theo trục Y.
- Sai số hướng
Xác định góc lệch ϕ giữa phương trình đường thẳng L và trục O’’Y’’ (x=0). Gọi n1(a1, b1) và n2(1, 0) lần lượt là vectơ pháp tuyến của 2 đường thẳng L và O’’Y’’, ta áp dụng công thức:
𝑐𝑜𝑠(ϕ) = | |𝑎1 (16)
𝑎12+𝑏12
Chú ý: - Nếuϕ>= 90 độ, góc giữa hai đường thẳng là 180- .ϕ - Nếuϕ< 90 độ, góc giữa hai đường thẳng xác định bằng .ϕ
Kết luận: Chương 2 đã chỉ ra đối tượng luận văn tập trung hướng tới cũng như trình bày chi tiết phương pháp để xây dựng lên thuật toán định vị vị trí dựa trên biên dạng đặc biệt, biên dạng VL.