基于規(guī)劃路徑約束的機(jī)器人定位方法

胡釗政; 許聰; 周哲; 鄧澤武

doi:10.11999/JEIT210984

基于規(guī)劃路徑約束的機(jī)器人定位方法

doi: 10.11999/JEIT210984

胡釗政^{1, 2, ,},
許聰^{1, 2},
周哲^{2, 3},
鄧澤武^{1, 2}

1.
武漢理工大學(xué)信息工程學(xué)院武漢 430070
2.
武漢理工大學(xué)智能交通系統(tǒng)研究中心武漢 430063
3.
武漢理工大學(xué)重慶研究院重慶 401120

基金項(xiàng)目: 國(guó)家自然科學(xué)基金(U1764262)，武漢市科學(xué)技術(shù)局企業(yè)技術(shù)創(chuàng)新項(xiàng)目(2020010601012165, 2020010602011973, 2020010602012003)，武漢理工大學(xué)重慶研究院科技創(chuàng)新研發(fā)項(xiàng)目(YF2021-04)

詳細(xì)信息

作者簡(jiǎn)介:
胡釗政：男，教授，研究方向?yàn)?D計(jì)算機(jī)視覺(jué)理論與應(yīng)用、智能車路系統(tǒng)、視覺(jué)與激光SLAM定位等

許聰：男，碩士生，研究方向?yàn)榧す釹LAM定位、機(jī)器人定位、多傳感器融合定位等

周哲：男，博士生，研究方向?yàn)榛谝曈X(jué)的機(jī)器人定位、多傳感器融合定位等

鄧澤武：男，碩士生，研究方向?yàn)榧す釹LAM定位、機(jī)器人定位、視覺(jué)定位等

通訊作者:
胡釗政　zzhu@whut.edu.cn

中圖分類號(hào): TP242
計(jì)量
- 文章訪問(wèn)數(shù): 900
- HTML全文瀏覽量: 148
- PDF下載量: 130
- 被引次數(shù): 0
出版歷程
- 收稿日期: 2021-09-15
- 修回日期: 2022-04-07
- 網(wǎng)絡(luò)出版日期: 2022-04-22
- 刊出日期: 2022-11-14

Robot Localization Based on Planned Path Constraints

HU Zhaozheng^{1, 2
, ,},
XU Cong^{1, 2},
ZHOU Zhe^{2, 3},
DENG Zewu^{1, 2}

1.
School of Information Engineering, Wuhan University of Technology, Wuhan 430070, China
2.
Intelligent Transportation Systems Research Center, Wuhan University of Technology, Wuhan 430063, China
3.
Chongqing Research Institute, Wuhan University of Technology, Chongqing 401120, China

Funds: The National Natural Science Foundation of China(U1764262), The Enterprise Technology Innovation Project of Wuhan Science and Technology Bureau(2020010601012165, 2020010602011973, 2020010602012003), The Scientific and Technological Innovation Research and Development Project of Chongqing Research Institute of Wuhan University of Technology(YF2021-04)

摘要

摘要: 路徑規(guī)劃是為機(jī)器人生成可行駛路徑以實(shí)現(xiàn)循跡的過(guò)程。因此，機(jī)器人的位置應(yīng)該位于或靠近規(guī)劃的行駛路徑。從而，路徑規(guī)劃可為機(jī)器人定位產(chǎn)生重要的約束。該文提出一種規(guī)劃路徑約束的位置概率圖 (PI-LPM)模型，該模型通過(guò)概率來(lái)表征機(jī)器人在整個(gè)地圖范圍內(nèi)所處的位置的可能性。其中，模型中概率密度函數(shù)是通過(guò)核密度估計(jì) (KDE)方法從表征規(guī)劃路徑的所有數(shù)據(jù)點(diǎn)生成。在所提出的PI-LPM模型基礎(chǔ)上，提出一種規(guī)劃路徑約束的機(jī)器人定位新算法 (RL-PPC)來(lái)提高機(jī)器人定位精度。在該方法中，應(yīng)用粒子濾波算法來(lái)融合所提出的PI-LPM模型和已有的傳感器定位方法。融合過(guò)程中，從PI-LPM模型中計(jì)算得到的概率是分配粒子權(quán)重的一個(gè)重要因素。實(shí)驗(yàn)中分別利用仿真數(shù)據(jù)和真實(shí)數(shù)據(jù)對(duì)所提出的模型與算法進(jìn)行驗(yàn)證。實(shí)驗(yàn)結(jié)果表明，所提RL-PPC算法可有效融合PI-LPM模型與主流的定位系統(tǒng)(如GPS和LiDAR定位系統(tǒng))，并顯著提高機(jī)器人定位的整體性能。
- 機(jī)器人定位 /
- 概率圖模型 /
- 路徑規(guī)劃 /
- 核密度估計(jì) /
- 粒子濾波
Abstract: Path planning is a step to generate a feasible path for a robot to track along. Locations of the robot are supposed to lie on or at least nearby the planned path, which can thus generate important constraints for robot localization. In this paper, a model, called Path-Induced Location Probability Map (PI-LPM), to exploit such constraint on robot localization is proposed. The proposed PI-LPM model is a Probability Density Function (PDF) over the entire map with the probability to describe the likelihood that the robot is located. The PDF is generated from all the points representing the path by applying the Kernel Density Estimation (KDE) method with each point as a sampling point. Based on the PI-LPM model, a Robot Localization from Planned Path Constraints (RL-PPC) method to enhance robot localization is proposed. In this method, particle filter is applied to fuse the develop PI-LPM model and existing localization methods, where the probability from PI-LPM is an important factor to assign weights to the particles. The proposed method is validated with both simulation and real data. In the experiment, the proposed PI-LPM model is integrated into both GPS and LiDAR based localization systems. Experimental results demonstrate that the RL-PPC method can effectively improve the over-all performance of robot localization.
- Robot localization /
- Probability map model /
- Path planning /
- Kernel Density Estimation (KDE) /
- Particle filter

HTML全文

圖 1 算法流程圖

下載: 全尺寸圖片幻燈片

圖 2 柵格地圖的繪制

下載: 全尺寸圖片幻燈片

圖 3 半橢圓形軌跡定位結(jié)果

下載: 全尺寸圖片幻燈片

圖 5 “S”形軌跡定位結(jié)果

下載: 全尺寸圖片幻燈片

圖 4 圓形軌跡定位結(jié)果

下載: 全尺寸圖片幻燈片

圖 6 二次規(guī)劃后的“S”形軌跡定位結(jié)果

下載: 全尺寸圖片幻燈片

圖 7 移動(dòng)機(jī)器人及其搭載的激光雷達(dá)

下載: 全尺寸圖片幻燈片

圖 8 場(chǎng)景1定位結(jié)果

下載: 全尺寸圖片幻燈片

圖 9 場(chǎng)景2定位結(jié)果

下載: 全尺寸圖片幻燈片

圖 10 場(chǎng)景1二次規(guī)劃定位結(jié)果

下載: 全尺寸圖片幻燈片

表 1 不同軌跡下RL-PPC方法定位誤差對(duì)比

軌跡	最大誤差(m)		平均誤差(m)		誤差1 m內(nèi)概率(%)
軌跡	GPS	RL-PPC	GPS	RL-PPC	GPS	RL-PPC
半橢圓	3.6545	2.0103	1.1822	0.5681	45.19	86.16
圓	3.6837	1.6219	1.1595	0.5316	44.78	88.57
“S”形	3.4489	1.9076	1.1742	0.6107	48.14	86.08

下載: 導(dǎo)出CSV

表 2 “S”形軌跡二次規(guī)劃前后RL-PPC定位誤差對(duì)比

軌跡	最大誤差(m)	平均誤差(m)	誤差1 m內(nèi)概率(%)
一次規(guī)劃“S”形	1.9076	0.6107	86.08
二次規(guī)劃“S”形	1.9044	0.6889	90.54

下載: 導(dǎo)出CSV

表 3 定位誤差與定位耗時(shí)對(duì)比

方法	定位耗時(shí)(ms/次)	最大誤差(m)	平均誤差(m)
文獻(xiàn)[18] 文獻(xiàn)[18]+本文融合算法	94 112	1.79 0.77	1.06 0.47
文獻(xiàn)[12] 文獻(xiàn)[12]+本文融合算法	56 65	1.60 0.73	0.96 0.45

下載: 導(dǎo)出CSV

參考文獻(xiàn)(18)

[1]	宿晨庚, 郭樹(shù)人, 劉旭楠, 等. 北斗三號(hào)基本系統(tǒng)空間信號(hào)質(zhì)量評(píng)估[J]. 電子與信息學(xué)報(bào), 2020, 42(11): 2689–2697. doi: 10.11999/JEIT190683 SU Chengeng, GUO Shuren, LIU Xunan, et al. Signal quality assessment of BDS-3 preliminary system[J]. Journal of Electronics &Information Technology, 2020, 42(11): 2689–2697. doi: 10.11999/JEIT190683
[2]	LEE B H, SONG J H, IM J H, et al. GPS/DR error estimation for autonomous vehicle localization[J]. Sensors, 2015, 15(8): 20779–20798. doi: 10.3390/s150820779
[3]	DU Xinxin and TAN K K. Vision-based approach towards lane line detection and vehicle localization[J]. Machine Vision and Applications, 2016, 27(2): 175–191. doi: 10.1007/s00138-015-0735-5
[4]	VALIENTE D, GIL A, PAYá L, et al. Robust visual localization with dynamic uncertainty management in omnidirectional SLAM[J]. Applied Sciences, 2017, 7(12): 1294. doi: 10.3390/app7121294
[5]	胡釗政, 劉佳蕙, 黃剛, 等. 融合WiFi、激光雷達(dá)與地圖的機(jī)器人室內(nèi)定位[J]. 電子與信息學(xué)報(bào), 2021, 43(8): 2308–2316. doi: 10.11999/JEIT200671 HU Zhaozheng, LIU Jiahui, HUANG Gang, et al. Integration of WiFi, laser, and map for robot indoor localization[J]. Journal of Electronics &Information Technology, 2021, 43(8): 2308–2316. doi: 10.11999/JEIT200671
[6]	WON P, BIGLARBEGIAN M, and MELEK W. Development of an effective docking system for modular mobile self-reconfigurable robots using extended Kalman filter and particle filter[J]. Robotics, 2015, 4(1): 25–49. doi: 10.3390/robotics4010025
[7]	WANG Fasheng, LIN Baowei, ZHANG Junxing, et al. Object tracking using langevin monte Carlo particle filter and locality sensitive histogram based likelihood model[J]. Computers & Graphics, 2018, 70: 214–223. doi: 10.1016/j.cag.2017.07.023
[8]	CAI Hao, HU Zhaozheng, HUANG Gang, et al. Integration of GPS, monocular vision, and high definition (HD) map for accurate vehicle localization[J]. Sensors, 2018, 18(10): 3270. doi: 10.3390/s18103270
[9]	曹祥紅, 李欣妍, 魏曉鴿, 等. 基于Dijkstra-ACO混合算法的應(yīng)急疏散路徑動(dòng)態(tài)規(guī)劃[J]. 電子與信息學(xué)報(bào), 2020, 42(6): 1502–1509. doi: 10.11999/JEIT190854 CAO Xianghong, LI Xinyan, WEI Xiaoge, et al. Dynamic programming of emergency evacuation path based on Dijkstra-ACO hybrid algorithm[J]. Journal of Electronics &Information Technology, 2020, 42(6): 1502–1509. doi: 10.11999/JEIT190854
[10]	YANG Hui, QI Jie, MIAO Yongchun, et al. A new robot navigation algorithm based on a Double-Layer ant algorithm and trajectory optimization[J]. IEEE Transactions on Industrial Electronics, 2019, 66(11): 8557–8566. doi: 10.1109/TIE.2018.2886798
[11]	李東方, 李科偉, 鄧宏彬, 等. 基于人工勢(shì)場(chǎng)與IB-LBM的機(jī)器蛇水中2D避障控制算法[J]. 機(jī)器人, 2018, 40(3): 346–359. doi: 10.13973/j.cnki.robot.170421 LI Dongfang, LI Kewei, DENG Hongbin, et al. The 2D aquatic obstacle avoidance control algorithm of the Snake-Like Robot based on artificial potential field and IB-LBM[J]. Robot, 2018, 40(3): 346–359. doi: 10.13973/j.cnki.robot.170421
[12]	SHAN Tixiao and ENGLOT B. LeGO-LOAM: Lightweight and ground-optimized Lidar Odometry and mapping on variable terrain[C]. 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Madrid, Spain, 2018: 4758–4765.
[13]	葉玨磊, 周志峰, 王立端, 等. 一種多線激光雷達(dá)與GNSS/INS系統(tǒng)標(biāo)定方法[J]. 激光與紅外, 2020, 50(1): 30–36. doi: 10.3969/j.issn.1001-5078.2020.01.006 YE Juelei, ZHOU Zhifeng, WANG Liduan, et al. A calibration method of Lidar and GNSS/INS system[J]. Laser &Infrared, 2020, 50(1): 30–36. doi: 10.3969/j.issn.1001-5078.2020.01.006
[14]	王任棟, 李華, 趙凱, 等. 基于核密度估計(jì)的城市動(dòng)態(tài)密集場(chǎng)景激光雷達(dá)定位[J]. 光學(xué)學(xué)報(bào), 2019, 39(5): 0528003. doi: 10.3788/AOS201939.0528003 WANG Rendong, LI Hua, ZHAO Kai, et al. Robust localization based on kernel density estimation in dynamic diverse city scenes using Lidar[J]. Acta Optica Sinica, 2019, 39(5): 0528003. doi: 10.3788/AOS201939.0528003
[15]	BI Haiyun, MA Jianwen, and WANG Fangjian. An improved particle filter algorithm based on ensemble Kalman filter and Markov chain monte Carlo method[J]. IEEE Journal of Selected Topics in Applied Earth Observations and Remote Sensing, 2015, 8(2): 447–459. doi: 10.1109/JSTARS.2014.2322096
[16]	劉海濤, 林艷明, 陳永華, 等. 基于遺傳算法的智能粒子濾波重采樣策略研究[J]. 電子與信息學(xué)報(bào), 2021, 43(12): 3459–3466. doi: 10.11999/JEIT200561 LIU Haitao, LIN Yanming, CHEN Yonghua, et al. A study on resampling strategy of intelligent particle filter based on genetic algorithm[J]. Journal of Electronics &Information Technology, 2021, 43(12): 3459–3466. doi: 10.11999/JEIT200561
[17]	徐公國(guó), 單甘霖, 段修生, 等. 基于馬爾科夫決策過(guò)程的多傳感器協(xié)同檢測(cè)與跟蹤調(diào)度方法[J]. 電子與信息學(xué)報(bào), 2019, 41(9): 2201–2208. doi: 10.11999/JEIT181129 XU Gongguo, SHAN Ganlin, DUAN Xiusheng, et al. Scheduling method based on Markov decision process for multi-sensor cooperative detection and tracking[J]. Journal of Electronics &Information Technology, 2019, 41(9): 2201–2208. doi: 10.11999/JEIT181129
[18]	ZHANG Ji and SINGH S. LOAM: Lidar Odometry and mapping in real-time[C]. Robotics: Science and Systems, Berkeley, USA, 2014: 1–10.