Real-Time Loop Closure in 2D LIDAR SLAM 缈昏瘧鍜屾€荤粨锛堜簩锛

风格不统一 提交于 2020-08-09 05:03:08

5.2 鍒嗘敮瀹氱晫鎵弿鍖归厤

鍍忕礌绾э紙pixel-accurate锛夊尮閰嶄紭鍖栧叕寮忎负锛欬br> 尉 鈭 = a r g m a x ( 尉 鈭 蠅 ) 鈭 k = 1 K M n e a r e s t ( T 尉 h k ) (BBS) \xi^*= argmax(\xi \in \omega)\sum_{k=1}^KM_{nearest}(T_\xi h_k) \tag{BBS} 鈭桙/span>=argmax(鈭圏/span>)k=1鈭慄/span>K鈥婞/span>Mnearest鈥婞/span>(T鈥婞/span>hk鈥婞/span>)(BBS)
涓婂紡涓?span> 蠅 \omega
琛ㄧず鎼滅储绐楀彛锛孅span> M n e a r e s t M_{nearest} Mnearest鈥婞/span>鏄皢杞崲鍚庣殑鍧愭爣M鎷撳睍鍒?span> R 2 R^2 R2
绌洪棿銆傞€氳繃灏嗚绠楀緱鍒扮殑鍧愭爣鑸嶅叆鍒拌窛绂绘渶杩戠殑鏍呮牸涓紝鐒跺悗灏嗚鏍呮牸鐐规嫇灞曞埌鍏跺搴旂殑Pixel锛堥檮杩戣窛绂绘渶杩戠殑鍏朵粬鏍呮牸鐐癸紵锛夈€傚尮閰嶇殑璐ㄩ噺鍙互杩涗竴姝ラ€氳繃鍏紡 C S CS CS鏉ヤ紭鍖栨彁楂樸€傞€傚綋鐨勯€夊彇閬嶅巻姝ラ暱锛屽彲浠ユ湁鏁堟彁楂樻晥鐜囥€傛枃涓€夋嫨褰撴縺鍏夐浄杈句互鏈€澶х殑鎺㈡祴璺濈 d m a x d_{max} dmax鈥婞/span>鎵弿鏃剁Щ鍔ㄤ笉瓒呰繃涓€涓儚绱犵殑瀹藉害( r r r, 涔熸槸涓€涓渶灏忓垎杈ㄧ巼鏍呮牸鐨勫搴︼紵) 涓洪檺鍒舵椂瀵瑰簲鐨勬柟鍚戣浆瑙掍负瑙掑害閬嶅巻姝ラ暱 未 胃 \delta_{\theta} 鈥婞/span>銆傚熀浜庝綑寮﹀畾鐞嗭紝鍙互寰楀埌锛欬span> d m a x = m a x ( k = 1 , . . . , K ) 鈭 鈭 h k 鈭 鈭 (6) d_{max} = max( k = 1, ..., K) ||h_k|| \tag6 dmax鈥婞/span>=max(k=1,...,K)鈭Ⅻ/span>鈭Ⅻ/span>hk鈥婞/span>鈭Ⅻ/span>鈭Ⅻ/span>(6) 未 胃 = a r c c o s ( 1 鈭 r 2 a d m a x 2 ) (7) \delta_{\theta }= arccos(1- \frac{r^2}{ad_{max}^2}) \tag7 鈥婞/span>=arccos(1鈭扅/span>admax2鈥婞/span>r2鈥婞/span>)(7)鎺ョ潃锛屽彲浠ュ熀浜庣粰瀹氱殑浣嶇Щ鍜岃搴︽悳绱㈢獥鍙eぇ灏忚绠楃Н鍒嗘鏁帮紝姣斿 W x = W y = 7 m 锛 W 胃 = 30 掳 W_x=W_y=7m锛學_\theta=30\degree Wx鈥婞/span>=Wy鈥婞/span>=7m锛孅/span>W鈥婞/span>=30: w x = W x r , w y = W y r , w 胃 = W 胃 r (8) w_x=\frac{W_x}{r}, w_y=\frac{W_y}{r}, w_\theta=\frac{W_\theta}{r} \tag8 wx鈥婞/span>=rWx鈥婞/span>鈥婞/span>,wy鈥婞/span>=rWy鈥婞/span>鈥婞/span>,w鈥婞/span>=rW鈥婞/span>鈥婞/span>(8)杩欎細浜х敓涓€涓湁闄愰泦鍚圏span> 蠅 \omega 锛屽熀浜庡湪褰撳墠鏈€鏂颁及璁′綅缃?span> 尉 0 \xi_0 0鈥婞/span>涓轰腑蹇冪殑鎼滅储绐楀彛锛欬span> 蠅 藟 = { 鈭 w x , . . . , w x } 脳 { 鈭 w y , . . . , w y } 脳 { 鈭 w 胃 , . . . , w 胃 } (9) \bar{\omega} = \{-w_x, ..., w_x \} \times \{-w_y,..., w_y \} \times \{-w_\theta,...,w_\theta \} \tag9 ={鈭扅/span>wx鈥婞/span>,...,wx鈥婞/span>}{鈭扅/span>wy鈥婞/span>,...,wy鈥婞/span>}{鈭扅/span>w鈥婞/span>,...,w鈥婞/span>}(9) 蠅 = { 尉 0 + ( r j x , r j y , 未 胃 j 胃 ) : ( j x , j y , j 胃 ) 鈭 蠅 藟 } (10) \omega = \{\xi_0 + (rj_x, rj_y, \delta_\theta j_\theta):(j_x, j_y, j_\theta) \in \bar{\omega}\} \tag{10} ={0鈥婞/span>+(rjx鈥婞/span>,rjy鈥婞/span>,鈥婞/span>j鈥婞/span>):(jx鈥婞/span>,jy鈥婞/span>,j鈥婞/span>)鈭圏/span>}(10)鍩轰簬浼犵粺鏂规硶杩涜鏆村姏閬嶅巻瀵绘壘鏈€浼樼殑鍖归厤 尉 鈭 \xi^* 鈭桙/span>瀹炵幇杈冧负绠€鍗曪紝鍙涓嬭堪绠楁硶1锛屼絾鏄獥鍙h秺澶э紝鎼滅储鏁堢巼瓒婃參锛欬br> 绠楁硶1:鏆村姏鍖归厤鏂规硶
鍦ㄨ繖閲屾彃鍏ュ浘鐗囨弿杩? width=
涓庝笂杩版柟娉曚笉鍚岀殑鏄紝浣滆€呴噰鐢ㄤ竴绉嶅垎鏀畾鐣岀殑鏂规硶鏉ュ疄鐜板湪鏇村ぇ鐨勬悳绱㈢獥鍙f湁鏁堣绠桙span> 尉 鈭 \xi^* 鈭桙/span>銆侟strong>绠楁硶2鎻忚堪浜嗘鏂规硶鐨勪竴鑸疄鐜拌繃绋嬨€傝鏂规硶棣栨鍦ㄦ贩鍚堟暣鍨嬬嚎鎬ц鍒掗棶棰樹腑浣跨敤[17]銆傚叧浜庤繖鏂归潰鐨勬枃鐚緝澶氾紝鍙互鍙傝[18]涓殑鏂囩尞姒傝堪銆侟br> 绠楁硶2锛氫竴鑸垎鏀畾鐣岀畻娉旤/strong>
鍦ㄨ繖閲屾彃鍏ュ浘鐗囨弿杩? width=璇ユ柟娉曠殑涓昏鎬濇兂鏄紝鍦ㄤ竴涓爲缁撴瀯閲岋紝鏍硅妭鐐硅〃绀烘墍鏈夊彲鑳界殑姒傜巼锛圏span> 蠅 \omega
锛夛紝瀛╁瓙鑺傜偣琛ㄧず姒傜巼瀛愰泦銆傛瘡涓妭鐐圭殑瀛╁瓙鑺傜偣缁勬垚鍙屼翰鑺傜偣鐨勪竴涓垎鏀紝鍥犳杩欎簺瀛╁瓙鑺傜偣涓€璧峰彲浠ヨ〃绀虹浉鍚岀殑姒傜巼闆嗗悎銆傚彾鑺傜偣鏄崟鐙殑锛屾瘡涓€涓〃绀哄崟鐙殑缁撴灉銆傜畻娉曟槸涓ユ牸鐨勶紝鍙鍐呴儴鑺傜偣c鐨勫緱鍒咟span> s c o r e ( c ) score(c) score(c)
鏄畠鍏冪礌锛堝叾瀛╁瓙锛夊緱鍒嗙殑涓婄晫锛屽垯鍜屾毚鍔涘尮閰嶆柟娉曚竴鏍峰彲浠ユ彁渚涚浉鍚岀殑缁撴灉銆傚湪杩欑鎯呭喌涓嬶紝鏃犺浠€涔堟椂鍊欎竴涓妭鐐规垚涓鸿竟鐣岋紝鍒欐瘮宸茬煡鐨勬渶浼樼殑缁撴灉鏇翠紭鐨勭粨鏋滀笉浼氬嚭鐜板湪鍏朵粬瀛愭爲涓€侟br>





鍏蜂綋鑰岃█锛岃鏂囦腑鐨勭畻娉曡繃绋嬪彲鍒嗕负浠ヤ笅姝ラ锛?锛夎妭鐐归€夋嫨锛圏em>node selection锛堺 2) 鍒嗘敮锛圏em>branching锛堺 3) 璁$畻涓婄晫锛圏em>computation of upper bounds锛夈€侟/p>

1锛夎妭鐐归€夋嫨锛圏em>Node Selection锛夛細浣滆€呴噰鐢ㄦ繁搴︿紭鍏堟悳绱紙DFS锛変綔涓烘棤鍏堥獙淇℃伅涓嬬殑榛樿鎼滅储鏂瑰紡锛氭鏃舵悳绱㈡晥鐜囬珮浣庡緢澶т竴閮ㄥ垎鍙栧喅浜庢爲鐨勭粨鏋勩€傝繖鐢变袱浠朵簨鎯呭喅瀹氾細涓€涓槸濂界殑涓婄晫锛 涓€涓槸濂界殑褰撳墠缁撴灉銆傚悗鑰呭彲浠ユ湁鍔╀簬娣卞害浼樺厛鎼滅储锛岃繖鏍疯兘澶熷緢蹇殑瀵硅澶氬彾瀛愯妭鐐硅繘琛岃瘎浼拌绠椼€傚洜涓哄湪鍥炵幆閲屽姞鍏ヤ綆鍖归厤搴︾殑绾︽潫鍙兘鍙嶈€屽鑷撮敊璇紝鎵€浠ヤ綔鑰呬篃璁句簡鏈€浣庡垎鏁伴槇鍊硷紝浣庝簬姝ら槇鍊肩殑鍖归厤缁撴灉涓嶄細鍦ㄤ紭鍖栬繃绋嬩腑浣跨敤銆傚疄闄呬腑鐨勫緱鍒嗗苟涓嶄細棰戠箒瓒呰繃姝ら槇鍊硷紝杩欒兘澶熼檷浣庤妭鐐归€夋嫨鎴栬€呭鎵惧垵濮嬪惎鍙戣В鐨勯噸瑕佺▼搴︼紙杩欐牱鍙互鎻愰珮鑺傜偣鐨勭瓫閫夋晥鐜囷級銆傚熀浜庢繁搴︿紭鍏堟悳绱㈤亶鍘嗗瀛愯妭鐐圭殑椤哄簭锛

标签
易学教程内所有资源均来自网络或用户发布的内容,如有违反法律规定的内容欢迎反馈
该文章没有解决你所遇到的问题?点击提问,说说你的问题,让更多的人一起探讨吧!