• <tr id="yyy80"></tr>
  • <sup id="yyy80"></sup>
  • <tfoot id="yyy80"><noscript id="yyy80"></noscript></tfoot>
  • 99热精品在线国产_美女午夜性视频免费_国产精品国产高清国产av_av欧美777_自拍偷自拍亚洲精品老妇_亚洲熟女精品中文字幕_www日本黄色视频网_国产精品野战在线观看 ?

    基于2D建圖算法的導(dǎo)航方法

    2022-04-21 07:19:44賀海育雍龍泉
    無線電工程 2022年4期
    關(guān)鍵詞:建圖里程計(jì)位姿

    李 娜,賀海育,賈 偉,雍龍泉

    (1.陜西理工大學(xué) 數(shù)學(xué)與計(jì)算機(jī)科學(xué)學(xué)院,陜西 漢中723000;2.西安航空學(xué)院 電子工程學(xué)院,陜西 西安 710017)

    0 引言

    近年來,在智能控制方面,對于即時(shí)定位與地圖構(gòu)建(Simultaneous Localization and Mapping,SLAM)算法的研究是一大熱點(diǎn)[1-8]。

    在2D-SLAM建圖與路徑規(guī)劃的應(yīng)用方面研究較多,文獻(xiàn)[9-11]中機(jī)器人的里程計(jì)信息進(jìn)行坐標(biāo)變換后,利用Gmapping建圖算法可以實(shí)現(xiàn)在室內(nèi)小范圍地圖的建立;文獻(xiàn)[12]提出了一種基于多傳感器融合的室內(nèi)建圖和定位算法;文獻(xiàn)[13]中采用基于高斯牛頓法(Guass-Newton)的Hector建圖算法,不需要里程計(jì)的情況下可以建立出高精度的地圖;文獻(xiàn)[14]提出了一種實(shí)時(shí)定位和建圖前端優(yōu)化的解決方法;文獻(xiàn)[15-19]基于機(jī)器人操作系統(tǒng)(Robot Operating System,ROS)平臺(tái),對導(dǎo)航機(jī)器人的整體架構(gòu)和路徑規(guī)劃進(jìn)行研究。以上研究大多需要大量的傳感器,造成在實(shí)際應(yīng)用中成本過高,且常用建圖算法較多而鮮有對比。在實(shí)際應(yīng)用中,最佳方案應(yīng)根據(jù)不同的環(huán)境選取不同的建圖算法,實(shí)現(xiàn)精確定位與導(dǎo)航。

    因此,本文使用ROS管理控制2D激光雷達(dá)和樹莓派來采集環(huán)境信息,對信息處理后分別利用Gmapping與Hector兩種算法建立二維柵格地圖,然后進(jìn)行路徑規(guī)劃,實(shí)現(xiàn)小車對未知環(huán)境的自由探索。實(shí)驗(yàn)證明,該系統(tǒng)不僅成本較低、結(jié)構(gòu)簡單,而且擴(kuò)展性以及適用性較強(qiáng)。

    1 小車硬件平臺(tái)設(shè)計(jì)

    首先需建立一輛智能監(jiān)測系統(tǒng)小車,設(shè)計(jì)小車的實(shí)時(shí)建圖與路徑規(guī)劃系統(tǒng)的結(jié)構(gòu)框架如圖1所示。

    圖1 系統(tǒng)結(jié)構(gòu)

    當(dāng)小車處于未知環(huán)境時(shí),首先在雷達(dá)節(jié)點(diǎn)中,計(jì)算機(jī)通過串口讀取到雷達(dá)所傳回的環(huán)境深度信息,然后在建圖節(jié)點(diǎn)中實(shí)時(shí)建立二維柵格地圖,并利用實(shí)時(shí)更新地圖節(jié)點(diǎn)不斷保存更新所建地圖。接著,將柵格地圖信息傳送到ROS系統(tǒng)中內(nèi)嵌的導(dǎo)航算法包(Navigation)進(jìn)行實(shí)時(shí)路徑規(guī)劃,再將所計(jì)算出的里程計(jì)信息給到底層控制節(jié)點(diǎn)(STM32),進(jìn)行小車運(yùn)動(dòng)控制,同時(shí)讀取底層里程計(jì)信息,構(gòu)成閉環(huán)控制。最后,在PC端利用遠(yuǎn)程登陸到樹莓派并在rviz可視化環(huán)境下進(jìn)行人機(jī)交互,可以在PC端設(shè)置目標(biāo)點(diǎn),觀察和記錄小車以及外部環(huán)境。

    本文所用的小車平臺(tái)由一個(gè)可解讀里程計(jì)信息的底盤、一個(gè)激光雷達(dá)和一臺(tái)Linux嵌入式計(jì)算機(jī)構(gòu)成,如圖2所示。激光雷達(dá)所使用的是杉川Delta-2A,其量程為8 m,掃面頻率6.2 Hz。所使用的小車主控制機(jī)器為Raspberry Pi 4B+,8 GB內(nèi)存版本。采用Ubuntu18.04操作系統(tǒng),并配置了ROS。

    圖2 小車平臺(tái)實(shí)物

    2 基于Hector算法建圖

    Hector無需里程計(jì),通過掃描匹配的方法進(jìn)行地圖創(chuàng)建,基于高斯牛頓法以求其最優(yōu)解,大大減少了算法所需的計(jì)算量。

    掃描匹配用當(dāng)前雷達(dá)所傳回的一幀數(shù)據(jù)與以往所建地圖數(shù)據(jù)構(gòu)造誤差函數(shù):

    (1)

    式中,ξ=(Px,Py,ψ)T是運(yùn)動(dòng)小車在世界坐標(biāo)系中xoy平面的位姿,Px和Py是在世界坐標(biāo)系的二維橫縱坐標(biāo),ψ是以世界坐標(biāo)系y軸為參考的旋轉(zhuǎn)角度,ξ為上一時(shí)刻運(yùn)動(dòng)小車的位姿或初始位姿;ξ*為當(dāng)前時(shí)刻最優(yōu)位姿。經(jīng)激光掃描所體現(xiàn)外部深度信息的點(diǎn)云坐標(biāo)轉(zhuǎn)化到世界坐標(biāo)系后的坐標(biāo)為:

    (2)

    式中,si=(si,x,si,y)T是激光掃描端點(diǎn)在雷達(dá)自身坐標(biāo)系下的坐標(biāo),si下標(biāo)序號(hào)代表激光點(diǎn)標(biāo)簽,可由激光雷達(dá)采集的原始數(shù)據(jù)的極坐標(biāo)轉(zhuǎn)化而來。其中右側(cè)第1個(gè)矩陣為旋轉(zhuǎn)矩陣,第3個(gè)矩陣為平移矩陣,此時(shí)用一組旋轉(zhuǎn)與平移即可完全描述出物體的運(yùn)動(dòng),函數(shù)M(Pm)代表坐標(biāo)點(diǎn)Pm在占用柵格地圖中的占用值,式(1)中坐標(biāo)點(diǎn)Pm為Si(ξ)的占用值,其值越大匹配效果越好,算法核心示意如圖3所示。

    圖3 Hector算法核心示意

    在Hector算法中,坐標(biāo)變換則顯得無比重要。根據(jù)傳感器傳回第1幀圖像來確定世界坐標(biāo)系。通過所采樣的第n幀圖像依次通過雷達(dá)運(yùn)動(dòng)來推算出每一幀圖像的坐標(biāo)變換,通過特征點(diǎn)檢查和疊加建立出完整的地圖,如圖4所示。

    (a)正常的室內(nèi)二維地圖

    利用Hector算法在教室中(矩形無障礙區(qū)域)進(jìn)行建圖實(shí)驗(yàn),可以發(fā)現(xiàn),在不使用里程計(jì)的情況下,當(dāng)傳感器有大幅度角度運(yùn)動(dòng)或行進(jìn)速度過快時(shí),圖像會(huì)出現(xiàn)運(yùn)動(dòng)畸變且Hector算法沒有地圖回環(huán),一旦定位丟失,Hector算法無法重新定位,也無法繼續(xù)建圖。

    3 基于Gmapping算法建圖

    Gmapping是基于濾波SLAM框架的常用開源算法。利用粒子濾波來實(shí)現(xiàn)定位[3]和建圖,在室內(nèi)或較小環(huán)境中的建圖效果更好。粒子濾波是基于蒙特卡羅方法的小車定位算法,其缺點(diǎn)是需要很多的樣本,算法復(fù)雜度很高,所需計(jì)算量較大,但卻可以適應(yīng)不同的誤差分布方式。

    在SALM中要解決的問題就是由控制數(shù)據(jù)和觀測數(shù)據(jù)來求位姿和地圖的聯(lián)合分布。即先進(jìn)行定位再進(jìn)行建圖。此算法會(huì)將第1幀所得到的數(shù)據(jù)作為小車的初始點(diǎn),然后小車進(jìn)行移動(dòng)時(shí)用所接收到的雷達(dá)數(shù)據(jù)與上一時(shí)刻所建的地圖進(jìn)行匹配,這樣可以對小車定位,用此時(shí)小車的位置可以建立出這一時(shí)刻的地圖。這樣重復(fù)下去即可一幀一幀地迭代建立出完整的地圖。算法核心流程如下:

    步驟1:輸入上一時(shí)刻的粒子群、最近時(shí)刻的里程計(jì)和雷達(dá)掃描信息,同時(shí)初始化當(dāng)前時(shí)刻的粒子群。

    步驟2:遍歷并獲取上一時(shí)刻粒子群中粒子所攜帶的位姿、權(quán)重和地圖信息。

    步驟3:通過里程計(jì)進(jìn)行位姿更新,用極大似然估計(jì)法可以求得局部極值。如果沒有找到局部極值則提議分布。

    步驟4:更新粒子位姿狀態(tài),然后使用觀測模型對位姿權(quán)重更新。若找到局部極值在其附近取k個(gè)位姿并認(rèn)為這k個(gè)位姿服從高斯分布。

    步驟5:計(jì)算這k個(gè)位姿的權(quán)重與均值,將均值歸一化后,求出k位姿的方差并將方差歸一化。

    步驟6:使用多元正態(tài)分布計(jì)算出新的位姿,計(jì)算該位姿粒子的權(quán)重。

    步驟7:更新地圖,更新粒子群并循環(huán)重復(fù)計(jì)算上一時(shí)刻的每一個(gè)粒子。

    步驟8:計(jì)算所有粒子權(quán)重的離散程度,并判斷是否超過閾值,是否進(jìn)行重采樣。

    在PC端中的rviz可視化環(huán)境下,可以觀察到建立的地圖,如圖5所示。

    (a)里程計(jì)不精準(zhǔn)時(shí)建圖效果

    在實(shí)驗(yàn)中,使用Gmapping算法對教室進(jìn)行建圖。從實(shí)際結(jié)果可以看出,在有里程計(jì)信息的(矩形無障礙區(qū)域)情況下,當(dāng)里程計(jì)不精準(zhǔn)時(shí)建圖效果并不理想,在地圖的后端與前端發(fā)生重影現(xiàn)象,但是可以精確計(jì)算里程計(jì)時(shí)(矩形有障礙區(qū)域)是可以建立精準(zhǔn)地圖的。由此可知,Gmapping算法嚴(yán)重依賴?yán)锍逃?jì),而無法適應(yīng)復(fù)雜的地面環(huán)境,且此算法在沒有里程計(jì)的情況下是無法建立地圖的。

    4 導(dǎo)航效果對比

    基于上述2種算法在不同的環(huán)境中,使用ROS中內(nèi)嵌的導(dǎo)航算法包(Navigation)進(jìn)行導(dǎo)航實(shí)驗(yàn),其中導(dǎo)航包要求輸入小車定位、地圖信息、里程計(jì)信息和雷達(dá)測量的點(diǎn)云數(shù)據(jù)以及小車、雷達(dá)與世界坐標(biāo)系的轉(zhuǎn)換關(guān)系。有了這些必需信息,導(dǎo)航包即可提供全局與局部的路徑規(guī)劃,從而實(shí)現(xiàn)導(dǎo)航。

    在配置好導(dǎo)航包后,分別在路面平整與路面不平整的情況下進(jìn)行建圖導(dǎo)航實(shí)驗(yàn),實(shí)驗(yàn)效果如圖6所示。

    (a)自由探索時(shí)最佳路徑

    經(jīng)實(shí)驗(yàn)驗(yàn)證,可以建立出精準(zhǔn)地圖并成功在圖中定位小車位置,且做出最優(yōu)路徑規(guī)劃(圖6中箭頭指向的綠線)。當(dāng)自由探索時(shí),小車可以自動(dòng)按照當(dāng)前地圖邊界向外界擴(kuò)展,并實(shí)時(shí)更新路徑,當(dāng)規(guī)定目標(biāo)點(diǎn)時(shí),小車可以進(jìn)行避障,且在此基礎(chǔ)上尋找最優(yōu)路徑。

    5 結(jié)束語

    通過2種算法進(jìn)行建圖導(dǎo)航,所設(shè)計(jì)的系統(tǒng)不僅成本較低,結(jié)構(gòu)簡單,而且擴(kuò)展性以及適用性較強(qiáng)。2種方法各有優(yōu)缺點(diǎn),其中Gmapping算法嚴(yán)重依賴?yán)锍逃?jì),無法適應(yīng)無人機(jī)在三維情況下飛行時(shí)的定位建圖及地面不平坦的區(qū)域,無回環(huán)即無法重新定位。在室外大場景粒子較多的情況下,非常消耗計(jì)算資源,很依賴計(jì)算力會(huì)造成在實(shí)際應(yīng)用中的成本過高,其更加適合于路面情況不復(fù)雜的巡檢工作。Hector算法基于解最小二乘問題,只用求出函數(shù)極值而不需要里程計(jì),而且可以適應(yīng)空中,手持實(shí)時(shí)記錄地圖信息或者地面不平坦的情況,但缺點(diǎn)在于對于雷達(dá)掃描頻率和幀率要求很高(40 Hz),并且需要估計(jì)三維空間下的6自由度機(jī)器人的位姿,這使得算法復(fù)雜度大幅增加。

    猜你喜歡
    建圖里程計(jì)位姿
    室內(nèi)退化場景下UWB雙基站輔助LiDAR里程計(jì)的定位方法
    視覺同步定位與建圖中特征點(diǎn)匹配算法優(yōu)化
    基于三輪全向機(jī)器人的室內(nèi)建圖與導(dǎo)航
    電子制作(2019年10期)2019-06-17 11:45:06
    一種單目相機(jī)/三軸陀螺儀/里程計(jì)緊組合導(dǎo)航算法
    一種基于多傳感融合的室內(nèi)建圖和定位算法
    基于模板特征點(diǎn)提取的立體視覺里程計(jì)實(shí)現(xiàn)方法
    機(jī)器人室內(nèi)語義建圖中的場所感知方法綜述
    基于共面直線迭代加權(quán)最小二乘的相機(jī)位姿估計(jì)
    基于CAD模型的單目六自由度位姿測量
    大角度斜置激光慣組與里程計(jì)組合導(dǎo)航方法
    双城市| 濮阳市| 仁化县| 高尔夫| 科尔| 思南县| 雷州市| 广河县| 友谊县| 雷波县| 卢湾区| 泉州市| 西充县| 娱乐| 海林市| 筠连县| 长治县| 义乌市| 醴陵市| 津市市| 封丘县| 海宁市| 来凤县| 锡林浩特市| 福安市| 分宜县| 巢湖市| 芜湖市| 富锦市| 石首市| 乐山市| 水城县| 苍溪县| 咸丰县| 桃园市| 鹤庆县| 唐河县| 锦州市| 伊宁县| 遂平县| 马边|