3. CS LÝ THU YT
3.1. Lý thuy tLie
3.1.2. Không gian t ip tuy n ca nhóm Lie
Vì c u trúc c a khơng gian ti p tuy n khơng ph thu c v trí x p x tuy n tính nên đ c g i chung là đ i s Lie (Lie algebra) c a nhóm Lie :
N u là s b c t do c a thì có s chi u là . Vì là khơng gian vector nên m i ph n t thu c đ u có th đ c bi u di n duy nh t thông qua m t vector
thu c vƠ ng c l i. Do đó, ta có th chuy n đ i qua l i gi a các ph n t thu c và thông qua 2 phép đ ng c u:
trong đó lƠ c s tiêu chu n c a và . Nói cách khác, chính là to
đ c a trong c s c a to thành t c s tiêu chu n c a .
chuy n đ i qua l i gi a các ph n t thu c và , ta s d ng 2 ánh x :
exp exp
15
Trong nhi u tr ng h p, ta c n quan tâm ph n t thu c không gian ti p tuy n
c c b thay vì khơng gian tồn c c . Do đó, và
đ c s d ng đ phân bi t lo i không gian ti p tuy n đang đ c nh c t i. thao tác tr c ti p trên khơng gian tuy n tính thay vì phi tuy n, ta đ nh ngh a thêm 2 ánh
x :
Exp Exp exp
Log Log log
Trong nhi u tr ng h p, ta c n chuy n đ i đ d i đư tính trong sang
vƠ ng c l i. thu n ti n, g i lƠ đ d i nguyên g c trong và lƠ bi u di n đ d i trong . C ng có th hi u ng c l i:
lƠ đ d i nguyên g c trong và lƠ bi u di n đ d i trong
. Ng i ta đư ch ng minh đ c r ng:
Exp Exp
bi u di n đ d i gi a các ph n t c a thông qua khái ni m vector tuy n tính truy n th ng, ta gi i thi u phép và . Vì phép h p trong khơng có tính giao hốn nên ta trình bày 2 phiên b n c a và : phiên b n cánh ph i và cánh trái. Phiên b n cánh ph i nh sau:
Exp Log
Vì nên ta nói đ c bi u di n c c b t i . Phiên b n cánh trái
nh sau:
Exp Log
Vì nên ta nói đ c bi u di n toàn c c. Vi c bi u di n đ d i c c b t i hay tồn c c khơng nh h ng đ n k t qu cu i cùng:
thu n ti n cho vi c chuy n đ i qua l i gi a và , ta s d ng m t ánh x tuy n tính trong , g i là phép li n k (adjoint operation) c a t i :
16
nh ngh a chi ti t v phép li n k n m ngoài gi i h n c a lu n v n. m c sau, ta s c th hoá phép li n k cho tr ng h p nhóm Lie ma tr n. Vì là to đ c a trong và là to đ c a trong , t n t i m t ma tr n chuy n to
đ (hay ma tr n li n k ) t sang : M t s tính ch t đáng chú ý c a ma tr n li n k nh sau: 3.1.3. o hàm trong nhóm Lie Xét m t ánh x gi a hai nhóm Lie và . T n d ng phép và , ta đ nh ngh a đ o hàm cánh ph i (hay ma tr n jacobian cánh ph i) c a t i nh sau: lim
trong đó và l n l t là s b c t do c a và . Chú ý phép chia trong bi u th c trên c n đ c hi u là phép l y đ o hàm (ma tr n jacobian) c a vector trong t i vector trong :
Vi c đ nh ngh a đ o hƠm trong nhóm Lie nh trên đ c l p v i c u trúc ph c t p c a các ph n t trong và . Vi phân c a t i đ c đ nh ngh a lƠ vi phơn trong các không gian ti p tuy n c c b t ng ng và . Ta c ng rút ra
bi u th c x p x Taylor b c nh t khi nh :
o hƠm cánh trái (ma tr n jacobian cánh trái) đ c tính t ng t : lim
17
o hàm cánh trái bi u di n vi phân c a t i trong các khơng gian ti p tuy n tồn c c t ng ng và . X p x Taylor b c nh t khi nh nh sau:
T n d ng tính ch t c a phép li n k , d dàng ch ng minh hai lo i đ o hàm liên h v i nhau thông qua bi u th c sau:
T đơy tr đi, n u không đ c p thêm, ta hi u đ o hƠm đang đ c nói đ n lƠ đ o hàm cánh ph i, phép và đ c nh c đ n thu c cánh ph i. M t quy t c quan tr ng khi th c hi n đ o hàm chính là quy t c chu i. C th , g i thêm m t ánh x khác
gi a hai nhóm Lie và . N u và thì . T tính x p x Taylor b c nh t, d dàng ch ng minh quy t c chu i:
Sau đơy, ta rút ra m t s k t qu h u ích khi th c hi n đ o hàm:
3.1.4. a t p ph c h p các nhóm Lie
M t đa t p ph c h p (c a các nhóm Lie) là h p c a đa t p thành ph n không t ng tác nhau. M i ph n t lƠ t p h p các ph n t t
các đa t p thƠnh ph n.
Nh m tho các tiên đ c a nhóm, ph n t đ n v , ph n t ngh ch đ o vƠ phép h p c a đa t p ph c h p đ c đ nh ngh a nh sau:
18
Phép Exp và Log đ c đ nh ngh a t ng t đ th ng nh t kí hi u:
Exp Exp Exp
Log Log Log
o hƠm trên các đa t p ph c h p có th đ c tính d a trên đ o hàm t ng kh i
đa t p thành ph n. Xét m t ánh x gi a hai đa t p ph c h p và . o hàm (cánh trái ho c ph i) c a t i có th đ c tính d a trên đ o hƠm trên
t ng đa t p thƠnh ph n:
3.1.5. Nhóm Lie ma tr n
Trong các nhóm Lie, nhóm Lie ma tr n (matrix Lie group) đóng vai trị quan tr ng trong các bài tốn robotics. Nhóm Lie ma tr n b c là nhóm Lie mà các ph n t là các ma tr n vuông kh ngh ch , phép h p c a nhóm chính là phép nhân ma tr n, ph n t đ n v là ma tr n đ n v � , ph n t nghch đ o là ma tr n
ngh ch đ o .
Gi s nhóm Lie ma tr n đ c tham s hoá b i bi n . T tính ch t ph n t ngh ch đ o c a nhóm, ta suy ra m i ph n t ph i có d ng:
Vì là ma tr n có kích th c nh nên phép nhân ma tr n gi a các ph n t thu c và ln có ngh a. Ánh x m exp và ánh x logarit log l n l t trùng v i phép m ma tr n và phép logarit ma tr n:
exp �
log
Th c hi n khai tri n Taylor và rút g n t phép m ma tr n, ta thu đ c:
19
K t qu trên đ c s d ng đ đ nh ngh a phép li n k cho nhóm Lie ma tr n: �d
i v i nhóm Lie ma tr n, ta rút ra m t s k t qu khi th c hi n đ o hàm:
3.2. Bi u di n v trí vƠ góc h ng
(Tài li u tham kh o chính c a ph n này là [50])
3.2.1. Bi u di n góc h ng 3 chi u
bi u di n góc h ng c a m t h to đ trong không gian 3 chi u, ta s d ng ma tr n xoay . Ng i ta đư ch ng minh là m t nhóm Lie ma tr n. Ta kí hi u đ i s Lie c a là . M i ph n t là m t ma tr n đ i x ng xiên (skew-symmetric matrix):
M t liên h thú v là chính là bi u di n góc-tr c xoay (axis-angle) c a phép
xoay th c hi n b i ma tr n xoay . Do đó, khi phơn tích , chính lƠ l ng xoay quanh tr c . Ng i ta đư ch ng minh phép Exp và Log trong có d ng t ng minh thông qua công th c Rodrigues kinh đi n. tính ma tr n xoay t , ta áp d ng phép Exp :
Exp sin cos
tính t , ta áp d ng phép Log :
Log sin cos tr
Chú ý khi l p trình, góc có th r t nh , khi n k t qu tr d li u rác ho c nan. x lý v n đ này, ta nên s d ng x p x Taylor n u nh :
20
3.2.2. Bi u di n t th 3 chi u
bi u di n v trí và góc h ng, hay cịn g i lƠ t th (pose), trong không gian 3 chi u, ta s d ng ma tr n thu n nh t :
Ng i ta đư ch ng minh là m t nhóm Lie ma tr n. Ta kí hi u đ i s Lie
c a là . M i ph n t đ u có d ng:
trong đó và l n l t là thành ph n to đ c a liên quan t i v trí và
góc h ng. Chú ý c ng chính lƠ bi u di n góc-tr c xoay (axis-angle) c a phép xoay th c hi n b i ma tr n xoay . Vì c u trúc đ c bi t c a ma tr n thu n nh t và ma tr n xoay, ta có th rút ra các cơng th c ng n g n sau khi tính ngh ch đ o và nhân ma tr n trên các ma tr n thu n nh t:
Công th c t ng minh cho phép Exp và Log trong nh sau:
Exp Exp Log LogLog
trong đó các ma tr n trung gian và đ c tính nh sau:
cos sin
sin cos
Chú ý khi l p trình, góc có th r t nh , khi n k t qu tr d li u rác ho c nan. x lý v n đ này, ta nên s d ng x p x Taylor n u nh :
cos sin
Ma tr n li n k c a phép li n k trong có d ng:
21
Ánh x này r t h u ích khi ta c n chuy n to đ các đi m trong không gian. Th c hi n l y đ o hƠm, ta thu đ c:
3.3. T i u bình ph ng c c ti u phi tuy n 3.3.1. D n gi i bài toán IRLS
(Tài li u tham kh o chính c a ph n này là [51])
Gi s ta c n t i u bi n thu c m t đa t p b t kì . Th c hi n phép đo cho ra
các th ng d (residual) ph thu c giá tr . K t qu đo t ng h p đ c bi u di n d i d ng vector:
Do nhi u c m bi n và nhi u y u t b t đnh khác, các th ng d phân b theo m t mơ hình xác su t, hay mơ hình đo l ng (measurement model) . Gi s vi c đo đ c các th ng d lƠ đ c l p t ng đôi m t, kh n ng (likelihood) x y ra k t qu đo khi bi t tr thành:
Áp d ng đnh lý Bayes, kh n ng h u nghi m (posteriori) x y ra khi bi t k t
qu đo tr thành:
Trong bi u th c trên, th hi n phân ph i tiên nghi m (prior distribution) c a
. Ta c n tìm sao cho t i đa kh n ng h u nghi m (Maximum a Posteriori ậ MAP):
argmax argmax argmax
S d ng logarit t nhiên, bài toán trên tr thành:
22 B qua , th c hi n gi i ph ng trình đ o hàm b ng 0: log log trong đó đ c đ nh ngh a nh sau: log Hàm đ c g i là hàm tr ng s , th hi n m c đ nh h ng c a t ng th ng
d trong quá trình t i u. Gi s const thì vi c gi i ph ng trình đ o hàm
trên t ng đ ng vi c gi i bƠi tốn bình ph ng c c ti u truy n th ng. Trong th c t , vì const nên đ bài toán g c đ c x p x thành bình ph ng c c ti u có tr ng s l p l i (iteratively reweighted least squares – IRLS) [52]: vi c tính tr ng s và th ng d đ c hoán đ i liên t c t i m i vòng l p đ n khi h i t :
argmin
3.3.2. Thi t k hàm tr ng s
đ m b o ch t l ng h i t cho IRLS, ta c n ch n m t mơ hình phân ph i phù h p đ bi u di n t t nh t t ng quan đ l n gi a các th ng d . Trong
tr ng h p lý t ng, tuân theo phân ph i Gauss, d n đ n const, t c bài toán tr v d ng bình ph ng c c ti u truy n th ng. Trong th c t , d li u ngo i lai (outlier) là nguyên nhân chính khi n l ch Gauss khá nhi u. tri t tiêu nh
h ng c a các outlier, phân ph i t (t-distribution) th ng đ c s d ng. Phân ph i t có th đ c coi là m r ng c a phân ph i Gauss vì ngồi kì v ng vƠ đ l ch chu n , phân ph i t s d ng thêm b c t do đ tu chnh đ dài c a “đuôi” phơn
ph i:
trong đó là hàm gamma. Khi , phân ph i t ti n t i phân ph i Gauss truy n th ng. Giá tr càng nh thì “đi” c a phân ph i càng dài, bao quát càng nhi u các
23
outlier, khi n phân ph i tr nên b n v ng h n. Mơ hình hố b ng phân ph i t,
ta thu đ c d ng hàm tr ng s nh sau:
M t ph ng pháp thông d ng đ l a ch n các thông s c a phân ph i t nh sau:
median mad kurtosiskurtosis
Tuy nhiên, trong nhi u tr ng h p, ph ng pháp trên không cho k t qu t i u.
đ m b o k t qu t t nh t, ta có th s d ng thêm các thu t tốn tìm ki m nhanh
nh Nelder-Mead [53] đ tìm kì v ng , đ l ch chu n và b c t do phù h p v i t p th ng d hi n t i .
Hình 3.2. Khi gi m, “đuôi” c a phân ph i (trái), th hi n rõ h n trên thang log
(ph i), tr nên dƠi h n, khi n phân ph i b n v ng h n v i outlier [54].
M t cách khác, đ n gi n h n, đ c i thi n ch t l ng IRLS đ i v i t p th ng d
ch a outlier là thi t k hàm tr ng s thông qua các công c c l ng M (M- estimator), hay còn g i là nhân b n v ng (robust kernel). M c tiêu c a nhân b n v ng không ph i tri t tiêu mà gi m nh h ng c a outlier trong quá trình t i u. Trong m t s tr ng h p, nhân b n v ng t ra h u hi u h n phơn ph i t trong vi c thi t k hàm tr ng s , nh t là khi t p th ng d nh , đ c hình thành thơng qua m t s gi i thu t tìm ki m r i r c ch không ph i l y m u liên t c nên không tuân theo các mơ hình xác su t truy n th ng.
Nguyên t c ho t đ ng c a nhân b n v ng r t đ n gi n: khi th ng d nh , nhân b n v ng cho giá tr nh L2; khi th ng d t ng, giá tr c a nhơn t ng ch m, th m chí
24
bão hoà, d n đ n nh h ng c a các outlier ti m tàng (có th ng d l n) b gi m. T c
đ làm gi m nh h ng c a outlier khác nhau s cho ra nhi u lo i nhân khác nhau. Trong gi i h n lu n v n, đ thu n ti n, ta s d ng nhân b n v ng t ng quát Barron [55] v i hƠm đ c tr ng nh sau:
trong đó là h s đnh d ng đ tu ch nh m c đ b n v ng c a nhân và là h s co dưn đ tu ch nh hình d ng c a nhơn quanh đi m . S d g i là nhân t ng quát vì b ng cách tu ch nh , ta suy ra nhi u nhân b n v ng quen thu c:
• (L2): lim
• (L1):
• (Geman-McClure):
Hình 3.3. HƠm đ c tr ng c a nhân t ng quát Barron (trái) và hàm tr ng s t ng ng v i các giá tr khác nhau [56].
25
4. THI T K GI I THU T 4.1. S đ kh i toàn h th ng 4.1. S đ kh i tồn h th ng
Hình 4.1. S đ kh i c a h th ng SLAM.
H th ng SLAM có nhi m v ti p nh n lu ng d li u thô t các c m bi n LiDAR vƠ camera, x lý đ c l ng t th 3D c a đ i t ng theo th i gian vƠ c p nh t b n đ . hoƠn thƠnh các nhi m v trên, h th ng th c thi các tác v sau: đ ngb c m bi n, c l ng t th vƠ qu n lý b n đ . D li u thô tr v t LiDAR là đám mơy
tia quét. D li u thô t camera lƠ nh bayer d ng GRBG, sau khi đ c debayer, chuy n sang grayscale vƠ c n ch nh đ lo i b bi n d ng thì đ c đ a vƠo h th ng. H th ng bao g m các module:
• Sensor Synchronizer: đ ng b 2 lu ng d li u thô t camera (khung nh) vƠ LiDAR (đám mơy) sao cho t i m i l n ng t, d li u t 2 c m bi n có timestamp sát nhau.
• Candidate Selector: l a ch n t p đi m nh ch n l c có gradient cao.
• Image Aligner: c l ng đ d i t th 3D d a vƠo x p ch ng gi a 2 khung
nh liên ti p. LiDAR Cloud Factor Photometric Factor Camera Cloud Map Portion Sensor Synchronizer Candidate Selector Cloud Registrator Image Aligner Windowed Optimizer Map Manager Image Candidate Points Map Guess Posex y z Pose x y z
26
• Cloud Registrator: c l ng t th 3D d a vƠo x p ch ng đám mơy hi n t i
vƠ b n đ .
• Windowed Optimizer: k t h p t th c l ng t camera vƠ LiDAR b ng đ
th nhơn t .
• Map Manager: c p nh t, truy xu t vƠ l u tr b n đ đám mơy.