傳感器都是基于底盤安裝的。因 此，base_link 和 basejaser 之間的變換矩陣為（X: 0.1m, y: 0.0m, z: 0.2m)。設(shè)置完 TF系統(tǒng)后，需要?jiǎng)?chuàng)建用于發(fā)布變換信息的節(jié)點(diǎn)，然后創(chuàng)建收聽變換信息的節(jié)點(diǎn)。ROS的TF 系統(tǒng)都可以很方便地進(jìn)行管理。
[0045] 步驟三：通過激光雷達(dá)獲取地圖信息，將激光雷達(dá)數(shù)據(jù)離散化。
[0046] 地圖獲取階段實(shí)際上就是激光雷達(dá)數(shù)據(jù)對環(huán)境進(jìn)行描述，這個(gè)階段也是將連續(xù)的 環(huán)境距離信息進(jìn)行離散化的階段。然后將這些離散的數(shù)據(jù)進(jìn)行匹配，將局部環(huán)境地圖融合 為全局環(huán)境地圖。
[0047] 在地圖獲取階段，首先就要定義單元柵格的大小。單元柵格越小，離散化程度越 低；相反，單元柵格越大，離散程度越高。離散化的基本思想為：從機(jī)器人當(dāng)前所在坐標(biāo)開 始，以單元柵格的尺寸向四周擴(kuò)散，使得平面內(nèi)鋪滿虛擬柵格，然后將激光雷達(dá)測距前端落 到相應(yīng)的柵格內(nèi)，由柵格的占有概率值來代表所有落到該柵格內(nèi)的測距前端數(shù)據(jù)。該算法 通過雙線性濾波實(shí)現(xiàn)，如圖5所示，圖中點(diǎn)P"是待插值的柵格地圖某點(diǎn)。因?yàn)闁鸥竦貓D其 自身離散的性質(zhì)限制了地圖所能達(dá)到的精度，所以要求先通過插值計(jì)算才能得到其地圖數(shù) 據(jù)，即將亞柵格單元的點(diǎn)通過雙線性插值轉(zhuǎn)化到柵格地圖。圖6為占有柵格地圖離散化的 效果。
[0048] 給定一個(gè)連續(xù)地圖上的某點(diǎn)坐標(biāo)Pm，占有值為M (Pm)，相應(yīng)的梯度為：
可以使用四個(gè)最近的坐標(biāo)PQQ.n近似：
[0049] 偏導(dǎo)數(shù)可以近似為：
[0050] 步驟四：將獲取的激光雷達(dá)信息進(jìn)行掃描匹配。
[0051] 完成激光雷達(dá)數(shù)據(jù)離散化后，得到了局部占有柵格地圖。然后通過掃描匹配階段 對激光雷達(dá)數(shù)據(jù)進(jìn)行掃描匹配，也就是通過局部地圖對移動(dòng)機(jī)器人位姿進(jìn)行估計(jì)。該算法 可以不使用里程計(jì)而進(jìn)行建圖，使算法可以應(yīng)用于操作人員手持建圖設(shè)備或者是使里程計(jì) 發(fā)生較大誤差的不平坦環(huán)境中。
[0052] 求移動(dòng)機(jī)器人位
需使得下式最小，也就是說要得到能夠使激 光掃描最佳的校準(zhǔn)：
[0053] 這里，SJ ζ )是激光雷達(dá)掃描端點(diǎn)SfGm，5"廣在全局坐標(biāo)系中的坐標(biāo)。它們是 機(jī)器人全局坐標(biāo)系位姿ζ的函數(shù)：
[0054] 方程ΜΒ^ζ))將返回坐標(biāo)SiU)處的地圖值。給定某個(gè)ζ的起始估計(jì)，估計(jì) Α ζ使得測量誤差最?。?br>[0055] 算法接下來對上式進(jìn)行處理。首先對ΜΒ^ζ+Δ ζ))進(jìn)行一階泰勒展開得到：
[0056] 該式可以通過對Δ ζ求偏導(dǎo)得到：
[0057] 此時(shí)，求△ ζ的問題轉(zhuǎn)換成為高斯牛頓方法求極小值的問題：
[0058] 其中，Η為海森矩陣：
[0059] ▽ M(Sje))為占有柵格地圖的偏導(dǎo)：
[0060] 步驟五：利用Zigbee協(xié)議構(gòu)建機(jī)器人與上位機(jī)間的無線通訊，實(shí)現(xiàn)上位機(jī)對機(jī)器 人狀態(tài)獲取與對激光雷達(dá)獲取的二維圖進(jìn)行實(shí)時(shí)監(jiān)控。
[0061] 在同一路由器下，配置操作員端的主機(jī)和移動(dòng)機(jī)器人的從機(jī)域名解析，從而完成 機(jī)器人和上位機(jī)通信的配置。此時(shí)，兩臺PC機(jī)進(jìn)行數(shù)據(jù)數(shù)據(jù)，只需要訂閱同一個(gè)話題即可。 通過該無線傳感器，構(gòu)建從操作員端電腦到移動(dòng)機(jī)器人端電腦的控制線路網(wǎng)絡(luò)。操作員可 以發(fā)送位姿指令，移動(dòng)機(jī)器人接收到通過無線網(wǎng)絡(luò)傳來的數(shù)據(jù)，然后在ROS的導(dǎo)航?？蚣?下進(jìn)行位姿跟蹤。
[0062] 步驟六：對機(jī)器人進(jìn)行同步定位與建圖。
[0063] Hector為R0S機(jī)器人操作系統(tǒng)的開源導(dǎo)航和建圖庫，如圖7所示。它主要用于城 市搜索和救援任務(wù)的移動(dòng)機(jī)器人系統(tǒng)中，給出了基于R0S的程序架構(gòu)與相應(yīng)組件。Hector 各個(gè)模塊可以進(jìn)行方便的獨(dú)立開發(fā)和整合，滿足不同配置移動(dòng)機(jī)器人和特定救援場合的使 用。其中最主要的組件為導(dǎo)航和建圖組件（he Ct〇r_slam)，構(gòu)建環(huán)境地圖不僅能夠?qū)C(jī)器 人所處的環(huán)境信息給操作人員，而且能夠給導(dǎo)航庫上層的路徑規(guī)劃組件提供可行區(qū)域和定 位信息，hector_slam只負(fù)責(zé)進(jìn)行2D建圖。hector_slam的組件包括用于執(zhí)行構(gòu)圖算法的 hector_mapping，為其他組件提供地圖數(shù)據(jù)服務(wù)的hector_map_server，將地圖信息轉(zhuǎn)化為 圖片信息的 hector_geotiff 和探索算法生成軌跡的 hector_trajectory_server。hector_ slam僅依靠激光雷達(dá)數(shù)據(jù)進(jìn)行建圖與定位，在道路特別崎嶇的時(shí)候，還可以使用慣性導(dǎo)航 對SLAM算法進(jìn)行補(bǔ)償，提供更高的定位精度。另外，如圖8所示，通過hector_elevation_ mapping，系統(tǒng)可以融合微軟的Kinect深度攝像頭，構(gòu)建2. f5D地圖信息。Kinect得到的點(diǎn) 云數(shù)據(jù)為2D地圖中相應(yīng)的柵格提供了高度信息及其方差。最后，hector_costmap可以融 合2. ?高度地圖與2D的占有柵格地圖構(gòu)建代價(jià)地圖。
[0064] 步驟七：設(shè)置機(jī)器人的路徑規(guī)劃算法。
[0065] 機(jī)器人路徑規(guī)劃算法使用Dijkstra算法。具體為對一個(gè)有權(quán)重的圖G進(jìn)行操作， 起點(diǎn)為S，V代表G中所有的定點(diǎn)的集合，E表示G中所有的邊的集合。（u，v)表示從頂點(diǎn)u 到頂點(diǎn)v的路徑，邊的權(quán)重為w:E - [0，°° ]，則w(u, v)是從頂點(diǎn)u到頂點(diǎn)v的非負(fù)代價(jià)， 即兩頂點(diǎn)之前的距離。一條路徑的代價(jià)值等于從路徑起點(diǎn)到終點(diǎn)所經(jīng)過的邊的代價(jià)值的 總和。Dijkstra算法可以找到圖G中任意兩個(gè)頂點(diǎn)如s、t的最低代價(jià)路徑，即最短路徑。 Dijkstra算法的流程圖如圖9所示。圖10給出了 Dijkstra算法的示例。算法具體步驟如 下。
[0066] St印1 :如圖10所示，算法開始時(shí)，開啟列表中只有起點(diǎn)S，0= {S}，且nbest=S，對S節(jié) 點(diǎn)的相鄰節(jié)點(diǎn)進(jìn)行遍歷，并且將S加入關(guān)閉列表，則，0= {2, 4, 1，5}，C={S}。相應(yīng)的從起點(diǎn)到 當(dāng)前點(diǎn)的代價(jià)函數(shù)分別為，f (2) =1，f (4) =1，f(l) =2, f (5) =4。代價(jià)函數(shù)最小的點(diǎn)位2和4， 任取其一，令nbest=2,而且此時(shí)反向指針均指向起點(diǎn)S。
[0067] Step2 :對節(jié)點(diǎn)2的相鄰節(jié)點(diǎn)進(jìn)行遍歷，此時(shí)0={4, 1，5}，C={S, 2}，而節(jié)點(diǎn)2的相鄰 節(jié)點(diǎn)只有S，而且S已經(jīng)在關(guān)閉列表中，算法繼續(xù)向下進(jìn)行。
[0068] Step3 :對當(dāng)前0={4, 1，5}中的代價(jià)函數(shù)最小的節(jié)點(diǎn)4進(jìn)行相鄰節(jié)點(diǎn)遍歷，nbest=4 ; 同樣，將4從開啟列表移到關(guān)閉列表，0={1，5}，C={S，2,4}，4的相鄰節(jié)點(diǎn)有S、3、5,其中 S已經(jīng)在關(guān)閉列表，不對其進(jìn)行處理，而在開啟列表中加入3。由于開啟列表中有5,而且 f (4)+c (4, 5)=1+2=3小于前面f (5) =4,需要更新開啟列表中5的代價(jià)函數(shù)，則f (5) =3,且5 的反向指針指向4。
[0069] 3七印4:此時(shí)開啟列表和關(guān)閉列表分別為0={5,3}，0={5,2,4}，11^=5。對節(jié)點(diǎn)5進(jìn) 行處理，5的相鄰節(jié)點(diǎn)有3個(gè)，其中4和S已經(jīng)在關(guān)閉列表中，不做處理，只剩終點(diǎn)G，因此把 終點(diǎn)G放入開啟列表，0={G，3}，C={S，2, 4, 5}。
[0070] St印5 :當(dāng)前代價(jià)函數(shù)最小的節(jié)點(diǎn)即為nbest=G，f(G)=f(5)+c(5，G)=5。將相應(yīng)節(jié)點(diǎn) 的反向指針取出，也就找到了一條從起點(diǎn)S到終點(diǎn)G的最短路徑S - 4 - 5 - G。
[0071] 步驟八：機(jī)器人進(jìn)行自主行駛，同時(shí)探索室內(nèi)未知環(huán)境。
[0072] 基于Hector SLAM的環(huán)境地圖構(gòu)建的基礎(chǔ)上，機(jī)器人進(jìn)行自主行駛建圖。移動(dòng)機(jī) 器人在探索未知區(qū)域的時(shí)候，需要進(jìn)行環(huán)境的建圖，同時(shí)也需要設(shè)置建圖規(guī)則，使得在相對 較短的時(shí)間內(nèi)能夠得到最大的環(huán)境地圖。
[0073] 本系統(tǒng)所使用的探索算法結(jié)合了現(xiàn)在常用的兩種策略。向離機(jī)器人最近的前端移 動(dòng)可以進(jìn)行快速的地圖獲取，擴(kuò)展未知區(qū)域。同時(shí)考慮現(xiàn)有地圖中前端的大小。前端較長 的地方，往往意味著通過這里能夠擴(kuò)展較大的未知區(qū)域。對兩個(gè)策略去權(quán)重進(jìn)行加權(quán)。移 動(dòng)機(jī)器人取其中能夠最快得到最大地圖的原則進(jìn)行地圖構(gòu)建，直到將所有的探索前端探索 完畢。
【主權(quán)項(xiàng)】
1. 一種基于ROS(機(jī)器人操作系統(tǒng)，RobotOperatingSystem)的移動(dòng)機(jī)器人室內(nèi)環(huán) 境探索系統(tǒng)與控制方法，包括上位機(jī)、改裝的iRobot機(jī)器人、UTM30LX激光雷達(dá)；通過采用 R0S的導(dǎo)航棧與基于HectorSLAM的自主建圖算法，實(shí)現(xiàn)移動(dòng)機(jī)器人對室內(nèi)環(huán)境的自主探 索并生成環(huán)境二維地圖。2. 權(quán)利要求1所述的一種基于R0S的移動(dòng)機(jī)器人室內(nèi)環(huán)境探索系統(tǒng)與控制方法，其特 征在于：包含如下步驟： 步驟1、以美國iRobot公司生產(chǎn)的iRobotCreate為移動(dòng)機(jī)器人平臺，進(jìn)行改裝與拓 展； 步驟2、采用R0S的導(dǎo)航棧（Navigation)來實(shí)現(xiàn)導(dǎo)航系統(tǒng)的具體框架，通過設(shè)置TF坐 標(biāo)變換系統(tǒng)、Costmap占有柵格地圖來完成導(dǎo)航棧的基本配置； 步驟3、通過激光雷達(dá)獲取地圖信息并進(jìn)行圖像信息的處理匹配； 步驟4、上位機(jī)對機(jī)器人狀態(tài)獲取與對激光雷達(dá)獲取的二維環(huán)境地圖信息進(jìn)行實(shí)時(shí)監(jiān) 控； 步驟5、利用R0S內(nèi)的Hector庫對機(jī)器人進(jìn)行同步定位與建圖； 步驟6、設(shè)置機(jī)器人的Dijkstra路徑規(guī)劃算法； 步驟7、機(jī)器人進(jìn)行自主行駛，同時(shí)探索室內(nèi)未知環(huán)境。
【專利摘要】本發(fā)明公開了一種基于ROS（機(jī)器人操作系統(tǒng)，Robot？Operating？System）的移動(dòng)機(jī)器人室內(nèi)環(huán)境探索系統(tǒng)與控制方法。該系統(tǒng)基于ROS，能夠?qū)崿F(xiàn)機(jī)器人對室內(nèi)環(huán)境的自主探索與定位。該系統(tǒng)主要采用iRobot差分驅(qū)動(dòng)底盤和UTM？30LX激光雷達(dá)。上位機(jī)可實(shí)時(shí)得到機(jī)器人的位置定位信息與室內(nèi)環(huán)境探索路徑軌跡，并由激光雷達(dá)的數(shù)據(jù)匹配得到由局部地圖連接形成的全局地圖。本發(fā)明提供了更加智能化的未知環(huán)境探索系統(tǒng)，為災(zāi)難環(huán)境救援提供了極大的便利與安全保障，實(shí)現(xiàn)了室內(nèi)環(huán)境感知的信息交互，具有很強(qiáng)的便捷性和實(shí)用性。
【IPC分類】G05D1/02
【公開號】CN105487535
【申請?zhí)枴緾N201410524184
【發(fā)明人】石成玉, 吳成東, 胡美玉, 陳建輝
【申請人】東北大學(xué)
【公開日】2016年4月13日
【申請日】2014年10月9日