Article(id=1200070544801493160, tenantId=1146029695717560320, journalId=1189918454225211397, issueId=1200070538191274203, articleNumber=null, orderNo=null, doi=10.20104/j.cnki.1674-6546.20230462, pmid=null, cstr=null, oa=null, hot=null, price=null, onlineType=0, articleFormat=0, articleType=null, articleTypeStr=null, receivedDate=null, receivedDateStr=null, revisedDate=1698508800000, revisedDateStr=2023-10-29, acceptedDate=null, acceptedDateStr=null, onlineDate=1764048714112, onlineDateStr=2025-11-25, pubDate=1705248000000, pubDateStr=2024-01-15, doiRegisterDate=null, doiRegisterDateStr=null, onlineIssueDate=1764048714112, onlineIssueDateStr=2025-11-25, onlineJustAcceptDate=null, onlineJustAcceptDateStr=null, onlineFirstDate=null, onlineFirstDateStr=null, sourceXml=null, magXml=null, createTime=1764048714112, creator=13701087609, updateTime=1764048714112, updator=13701087609, issue=Issue{id=1200070538191274203, tenantId=1146029695717560320, journalId=1189918454225211397, year='2024', volume='', issue='1', pageStart='1', pageEnd='48', issueExtLink='null', onlineDate='null', pubDate='null', beforeIssueId=null, nextIssueId=null, price=null, status=1, issueComplete=1, articleOrder=1, issueType=-1, specialIssue=null, createTime=1764048712537, creator=13701087609, updateTime=1764049067190, updator=13701087609, preIssue=null, nextIssue=null, ext={EN=IssueExt(id=1200072025768293209, tenantId=1146029695717560320, journalId=1189918454225211397, issueId=1200070538191274203, language=EN, specialIssueTitle=, coverIllustrator=null, specialIssueEditor=, specialIssueAbout=), CN=IssueExt(id=1200072025768293210, tenantId=1146029695717560320, journalId=1189918454225211397, issueId=1200070538191274203, language=CN, specialIssueTitle=, coverIllustrator=null, specialIssueEditor=, specialIssueAbout=)}, issueFiles=null}, startPage=19, endPage=24, ext={EN=ArticleExt(id=1200070545371918516, articleId=1200070544801493160, tenantId=1146029695717560320, journalId=1189918454225211397, language=EN, title=Autonomous Vehicle Location Based on Prior Map/GNSS/IMU Fusion, columnId=1200070539592167435, journalTitle=Automotive Engineer, columnName=Special Topic on Autonomous Driving Environment Perception and Positioning Technology at Chongqing Jiaotong University, runingTitle=null, highlight=null, articleAbstract=

To solve the problem that a single sensor cannot provide continuous and stable position information. In this paper, an innovative method is proposed to build a high-precision positioning system by integrating three kinds of data, namely global satellite navigation system, Inertial Measurement Unit (IMU) and prior map point cloud matching, through the Error State Kalman Filter (ESKF) algorithm. At the same time, by introducing the correction matrix method, the pose problem can be corrected to avoid the system failing to obtain effective observations in the case of Global Navigation Satellite System (GNSS) failure or prior map matching delay, etc., so that the system can maintain stability and be in a high-precision state. The experimental results show that the proposed method can still maintain high precision in the case of failure of the observed value. Compared with the traditional single sensor positioning method and the common GNSS/IMU combined positioning method, the proposed method has higher robustness and reliability, and can better meet the positioning requirements of autonomous vehicles in complex environments.

, correspAuthors=null, authorNote=null, correspAuthorsNote=null, copyrightStatement=null, copyrightOwner=null, extLink=null, articleAbsUrl=null, sourceXml=null, magXml=null, pdfUrl=null, pdf=null, pdfFileSize=null, pdfExtLink=null, richHtmlUrl=null, mobilePdfUrl=null, reviewReport=null, pdfFirstPage=null, abstractGraph=null, abstractGraphContent=null, abstractVideo=null, citation=null, cebUrl=null, magXmlContent=null, mapNumber=null, authorCompany=null, fund=null, authors=null, authorsList=Huachuan Xiong), CN=ArticleExt(id=1200070547963998619, articleId=1200070544801493160, tenantId=1146029695717560320, journalId=1189918454225211397, language=CN, title=基于先验地图/GNSS/IMU融合的自动驾驶车辆定位系统, columnId=1200070539772522510, journalTitle=汽车工程师, columnName=重庆交通大学自动驾驶环境感知与定位技术专题, runingTitle=null, highlight=null, articleAbstract=

针对仅依靠单一的传感器无法提供持续稳定的位置信息的问题,利用误差卡尔曼滤波(ESKF)算法,融合全球卫星导航系统(GNSS)、惯性测量单元(IMU)和先验地图点云数据,构建了高精度定位系统,并通过引入矩阵校正方法,避免GNSS失效或先验地图匹配延迟等情况下系统无法获取有效观测值以修正位姿的问题,使系统保持稳定并处于高精度状态。试验结果表明:在观测值失效等情况下,该方法仍然能够保持高精度的定位状态;相较于传统的单一传感器定位方法以及常见的GNSS/IMU组合定位方法,所提出的方法具有更高的鲁棒性和可靠性,能够更好地满足自动驾驶汽车在复杂环境中的定位需求。

, correspAuthors=null, authorNote=null, correspAuthorsNote=null, copyrightStatement=null, copyrightOwner=null, extLink=null, articleAbsUrl=null, sourceXml=iCtqLCL+ixb/m3wzplkCzQ==, magXml=h0Hwrb7InLWkdaEG/8b28g==, pdfUrl=null, pdf=7bRIwpMN4nph2IPiGfEvbA==, pdfFileSize=2511044, pdfExtLink=null, richHtmlUrl=null, mobilePdfUrl=null, reviewReport=null, pdfFirstPage=null, abstractGraph=whXpFaDUBCuoIU1+Uw3s7w==, abstractGraphContent=null, abstractVideo=null, citation=null, cebUrl=null, magXmlContent=AiSW1Jvc6tIWEL6fBHilYw==, mapNumber=null, authorCompany=null, fund=null, authors=null, authorsList=熊华川)}, authors=[Author(id=1200070548458926551, tenantId=1146029695717560320, journalId=1189918454225211397, articleId=1200070544801493160, orderNo=0, firstName=null, middleName=null, lastName=null, nameCn=null, orcid=null, stid=null, country=null, authorPic=null, dead=0, email=null, emailSecond=null, emailThird=null, correspondingAuthor=0, authorType=1, ext={EN=AuthorExt(id=1200070548626698730, tenantId=1146029695717560320, journalId=1189918454225211397, articleId=1200070544801493160, authorId=1200070548458926551, language=EN, stringName=Huachuan Xiong, firstName=Huachuan, middleName=null, lastName=Xiong, prefix=null, suffix=null, authorComment=null, nameInitials=null, affiliation=null, department=null, xref=null, address= Chongqing Jiaotong University, 400074, bio=null, bioImg=null, bioContent=null, aboutCorrespAuthor=null), CN=AuthorExt(id=1200070548760916475, tenantId=1146029695717560320, journalId=1189918454225211397, articleId=1200070544801493160, authorId=1200070548458926551, language=CN, stringName=熊华川, firstName=null, middleName=null, lastName=null, prefix=null, suffix=null, authorComment=null, nameInitials=null, affiliation=null, department=null, xref=null, address=重庆交通大学, 重庆 400074, bio=null, bioImg=null, bioContent=null, aboutCorrespAuthor=null)}, companyList=[AuthorCompany(id=1200070548228239800, tenantId=1146029695717560320, journalId=1189918454225211397, articleId=1200070544801493160, xref=null, ext=[AuthorCompanyExt(id=1200070548240822715, tenantId=1146029695717560320, journalId=1189918454225211397, articleId=1200070544801493160, companyId=1200070548228239800, language=EN, country=null, province=null, city=null, postcode=null, companyName=null, departmentName=null, remark= Chongqing Jiaotong University, 400074), AuthorCompanyExt(id=1200070548312125892, tenantId=1146029695717560320, journalId=1189918454225211397, articleId=1200070544801493160, companyId=1200070548228239800, language=CN, country=null, province=null, city=null, postcode=null, companyName=null, departmentName=null, remark=重庆交通大学, 重庆 400074)])])], keywords=[Keyword(id=1200070548928688652, tenantId=1146029695717560320, journalId=1189918454225211397, articleId=1200070544801493160, language=EN, orderNo=1, keyword=Priori map), Keyword(id=1200070549037740571, tenantId=1146029695717560320, journalId=1189918454225211397, articleId=1200070544801493160, language=EN, orderNo=2, keyword=HD positioning), Keyword(id=1200070549159375400, tenantId=1146029695717560320, journalId=1189918454225211397, articleId=1200070544801493160, language=EN, orderNo=3, keyword=Multi-sensor fusion), Keyword(id=1200070549276815925, tenantId=1146029695717560320, journalId=1189918454225211397, articleId=1200070544801493160, language=EN, orderNo=4, keyword=Error State Kalman Filter (ESKF)), Keyword(id=1200070549398450749, tenantId=1146029695717560320, journalId=1189918454225211397, articleId=1200070544801493160, language=CN, orderNo=1, keyword=先验地图), Keyword(id=1200070549549445711, tenantId=1146029695717560320, journalId=1189918454225211397, articleId=1200070544801493160, language=CN, orderNo=2, keyword=高精定位), Keyword(id=1200070549671080540, tenantId=1146029695717560320, journalId=1189918454225211397, articleId=1200070544801493160, language=CN, orderNo=3, keyword=多传感器融合), Keyword(id=1200070549763355236, tenantId=1146029695717560320, journalId=1189918454225211397, articleId=1200070544801493160, language=CN, orderNo=4, keyword=误差卡尔曼滤波)], refs=[Reference(id=1200070552875529106, tenantId=1146029695717560320, journalId=1189918454225211397, articleId=1200070544801493160, doi=null, pmid=null, pmcid=null, year=2005, volume=169, issue=2, pageStart=146, pageEnd=149, url=null, language=null, rfNumber=[1], rfOrder=0, authorNames=OATES T, journalName=Artificial Intelligence, refType=null, unstructuredReference=OATES T. Book Review: Introduction to Autonomous Mobile Robots[J]. Artificial Intelligence, 2005, 169(2): 146-149., articleTitle=Book Review: Introduction to Autonomous Mobile Robots, refAbstract=null), Reference(id=1200070552984581014, tenantId=1146029695717560320, journalId=1189918454225211397, articleId=1200070544801493160, doi=null, pmid=null, pmcid=null, year=2023, volume=54, issue=18, pageStart=165, pageEnd=167, url=null, language=null, rfNumber=[2], rfOrder=1, authorNames=朱洪达, 罗强, 辛琪, journalName=南方农机, refType=null, unstructuredReference=朱洪达, 罗强, 辛琪. 基于多传感器融合的定位技术概述[J]. 南方农机, 2023, 54(18): 165-167., articleTitle=基于多传感器融合的定位技术概述, refAbstract=null), Reference(id=1200070553139770276, tenantId=1146029695717560320, journalId=1189918454225211397, articleId=1200070544801493160, doi=null, pmid=null, pmcid=null, year=2023, volume=54, issue=18, pageStart=165, pageEnd=167, url=null, language=null, rfNumber=[2], rfOrder=2, authorNames=ZHU H D, LUO Q, XIN Q, journalName=South Farm Machinery, refType=null, unstructuredReference=ZHU H D, LUO Q, XIN Q. Overview of Positioning Technology Based on Multi-Sensor Fusion[J]. South Farm Machinery, 2023, 54(18): 165-167., articleTitle=Overview of Positioning Technology Based on Multi-Sensor Fusion, refAbstract=null), Reference(id=1200070553286570921, tenantId=1146029695717560320, journalId=1189918454225211397, articleId=1200070544801493160, doi=null, pmid=null, pmcid=null, year=2023, volume=38, issue=2, pageStart=75, pageEnd=81, url=null, language=null, rfNumber=[3], rfOrder=3, authorNames=李庆成, 王玉林, 逯宇, journalName=青岛大学学报(工程技术版), refType=null, unstructuredReference=李庆成, 王玉林, 逯宇, 等. 基于IMU/GNSS/车速传感器的矿用车定位系统研究[J]. 青岛大学学报(工程技术版), 2023, 38(2): 75-81., articleTitle=基于IMU/GNSS/车速传感器的矿用车定位系统研究, refAbstract=null), Reference(id=1200070553445954483, tenantId=1146029695717560320, journalId=1189918454225211397, articleId=1200070544801493160, doi=null, pmid=null, pmcid=null, year=2023, volume=38, issue=2, pageStart=75, pageEnd=81, url=null, language=null, rfNumber=[3], rfOrder=4, authorNames=LI Q C, WANG Y L, LU Y, journalName=Journal of Qingdao University (Engineering and Technology Edition), refType=null, unstructuredReference=LI Q C, WANG Y L, LU Y, et al. Research on Positioning System of Mining Vehicle Based on IMU/GNSS/Speed Sensor[J]. Journal of Qingdao University (Engineering and Technology Edition), 2023, 38(2): 75-81., articleTitle=Research on Positioning System of Mining Vehicle Based on IMU/GNSS/Speed Sensor, refAbstract=null), Reference(id=1200070553580172220, tenantId=1146029695717560320, journalId=1189918454225211397, articleId=1200070544801493160, doi=null, pmid=null, pmcid=null, year=2015, volume=29, issue=null, pageStart=82, pageEnd=94, url=null, language=null, rfNumber=[4], rfOrder=5, authorNames=MUSAVI N, KEIGHOBADI J, journalName=Applied Soft Computing Journal, refType=null, unstructuredReference=MUSAVI N, KEIGHOBADI J. Adaptive Fuzzy Neuro-Observer Applied to Low Cost INS/GPS[J]. Applied Soft Computing Journal, 2015, 29: 82-94., articleTitle=Adaptive Fuzzy Neuro-Observer Applied to Low Cost INS/GPS, refAbstract=null), Reference(id=1200070553685029826, tenantId=1146029695717560320, journalId=1189918454225211397, articleId=1200070544801493160, doi=null, pmid=null, pmcid=null, year=2011, volume=51, issue=1, pageStart=122, pageEnd=125, url=null, language=null, rfNumber=[5], rfOrder=6, authorNames=李鹏, 陆明泉, 冯振明, journalName=清华大学学报(自然科学版), refType=null, unstructuredReference=李鹏, 陆明泉, 冯振明. GNSS/IMU组合导航观测量丢弃策略[J]. 清华大学学报(自然科学版), 2011, 51(1): 122-125., articleTitle=GNSS/IMU组合导航观测量丢弃策略, refAbstract=null), Reference(id=1200070553823441867, tenantId=1146029695717560320, journalId=1189918454225211397, articleId=1200070544801493160, doi=null, pmid=null, pmcid=null, year=2011, volume=51, issue=1, pageStart=122, pageEnd=125, url=null, language=null, rfNumber=[5], rfOrder=7, authorNames=LI P, LU M Q, FENG Z M, journalName=Journal of Tsinghua University (Natural Science Edition), refType=null, unstructuredReference=LI P, LU M Q, FENG Z M. Measurement Discarding Strategy for GNSS/IMU Integration[J]. Journal of Tsinghua University (Natural Science Edition), 2011, 51(1): 122-125., articleTitle=Measurement Discarding Strategy for GNSS/IMU Integration, refAbstract=null), Reference(id=1200070553936688081, tenantId=1146029695717560320, journalId=1189918454225211397, articleId=1200070544801493160, doi=null, pmid=null, pmcid=null, year=2004, volume=null, issue=9, pageStart=808, pageEnd=811, url=null, language=null, rfNumber=[6], rfOrder=8, authorNames=缪玲娟, 李春明, 郭振西, journalName=北京理工大学学报, refType=null, unstructuredReference=缪玲娟, 李春明, 郭振西, 等. 陆用捷联惯导系统/里程计自主式组合导航技术[J]. 北京理工大学学报, 2004(9): 808-811., articleTitle=陆用捷联惯导系统/里程计自主式组合导航技术, refAbstract=null), Reference(id=1200070554142208993, tenantId=1146029695717560320, journalId=1189918454225211397, articleId=1200070544801493160, doi=null, pmid=null, pmcid=null, year=2004, volume=null, issue=9, pageStart=808, pageEnd=811, url=null, language=null, rfNumber=[6], rfOrder=9, authorNames=MIAO L J, LI C M, GUO Z X, journalName=Transactions of Beijing Institute of Technology, refType=null, unstructuredReference=MIAO L J, LI C M, GUO Z X, et al. Independently Integrated Navigation System of SINS and Distance-Transfer-Unit for Land Vehicles[J]. Transactions of Beijing Institute of Technology, 2004(9): 808-811., articleTitle=Independently Integrated Navigation System of SINS and Distance-Transfer-Unit for Land Vehicles, refAbstract=null), Reference(id=1200070554360312813, tenantId=1146029695717560320, journalId=1189918454225211397, articleId=1200070544801493160, doi=null, pmid=null, pmcid=null, year=2011, volume=null, issue=null, pageStart=null, pageEnd=null, url=null, language=null, rfNumber=[7], rfOrder=10, authorNames=MOOSMANN F, STILLER C, journalName=2011 IEEE Intelligent Vehicles Symposium (IV), refType=null, unstructuredReference=MOOSMANN F, STILLER C. Velodyne SLAM[C]// 2011 IEEE Intelligent Vehicles Symposium (IV). Baden-Baden, Germany: IEEE, 2011., articleTitle=Velodyne SLAM, refAbstract=null), Reference(id=1200070554540667895, tenantId=1146029695717560320, journalId=1189918454225211397, articleId=1200070544801493160, doi=null, pmid=null, pmcid=null, year=2013, volume=null, issue=null, pageStart=null, pageEnd=null, url=null, language=null, rfNumber=[8], rfOrder=11, authorNames=YANG J L, LI H D, JIA Y D, journalName=2013 IEEE International Conference on Computer Vision, refType=null, unstructuredReference=YANG J L, LI H D, JIA Y D. Go-ICP: Solving 3D Registration Efficiently and Globally Optimally[C]// 2013 IEEE International Conference on Computer Vision. Sydney, NSW, Australia: IEEE, 2013., articleTitle=Go-ICP: Solving 3D Registration Efficiently and Globally Optimally, refAbstract=null), Reference(id=1200070554687467522, tenantId=1146029695717560320, journalId=1189918454225211397, articleId=1200070544801493160, doi=null, pmid=null, pmcid=null, year=2016, volume=88, issue=null, pageStart=104, pageEnd=115, url=null, language=null, rfNumber=[9], rfOrder=12, authorNames=DROESCHEL D, SCHWARZ M, BEHNKE S, journalName=Robotics & Autonomous Systems, refType=null, unstructuredReference=DROESCHEL D, SCHWARZ M, BEHNKE S. Continuous Mapping and Localization for Autonomous Navigation in Rough Terrain Using a 3D Laser Scanner[J]. Robotics & Autonomous Systems, 2016, 88: 104-115., articleTitle=Continuous Mapping and Localization for Autonomous Navigation in Rough Terrain Using a 3D Laser Scanner, refAbstract=null), Reference(id=1200070554779742221, tenantId=1146029695717560320, journalId=1189918454225211397, articleId=1200070544801493160, doi=null, pmid=null, pmcid=null, year=2023, volume=null, issue=null, pageStart=73, pageEnd=null, url=null, language=null, rfNumber=[10], rfOrder=13, authorNames=高翔, journalName=自动驾驶与机器人中的SLAM技术: 从理论到实践, refType=null, unstructuredReference=高翔. 自动驾驶与机器人中的SLAM技术: 从理论到实践[M]. 北京. 电子工业出版社, 2023: 73-74., articleTitle=null, refAbstract=null), Reference(id=1200070554888794133, tenantId=1146029695717560320, journalId=1189918454225211397, articleId=1200070544801493160, doi=null, pmid=null, pmcid=null, year=2023, volume=null, issue=null, pageStart=73, pageEnd=null, url=null, language=null, rfNumber=[10], rfOrder=14, authorNames=GAO X, journalName=SLAM in Autonomous Driving and Robotics: From Theory to Practice, refType=null, unstructuredReference=GAO X. SLAM in Autonomous Driving and Robotics: From Theory to Practice[M]. Beijing: Electronics Industry Press, 2023: 73-74., articleTitle=null, refAbstract=null), Reference(id=1200070554997846050, tenantId=1146029695717560320, journalId=1189918454225211397, articleId=1200070544801493160, doi=null, pmid=null, pmcid=null, year=2017, volume=null, issue=null, pageStart=null, pageEnd=null, url=https://arxiv.org/abs/1711.02508, language=null, rfNumber=[11], rfOrder=15, authorNames=SOLÀ J, journalName=null, refType=null, unstructuredReference=SOLÀ J. Quaternion Kinematics for the Error-State Kalman Filter[EB/OL]. (2017-11-03) [2023-10-29]. https://arxiv.org/abs/1711.02508, articleTitle=Quaternion Kinematics for the Error-State Kalman Filter, refAbstract=null), Reference(id=1200070555111092267, tenantId=1146029695717560320, journalId=1189918454225211397, articleId=1200070544801493160, doi=null, pmid=null, pmcid=null, year=2010, volume=37, issue=5, pageStart=1209, pageEnd=1212, url=null, language=null, rfNumber=[12], rfOrder=16, authorNames=吴赛成, 秦石乔, 王省书, journalName=中国激光, refType=null, unstructuredReference=吴赛成, 秦石乔, 王省书, 等. 基于姿态解算的z向激光陀螺零偏估计方法[J]. 中国激光, 2010, 37(5): 1209-1212., articleTitle=基于姿态解算的z向激光陀螺零偏估计方法, refAbstract=null), Reference(id=1200070555245310004, tenantId=1146029695717560320, journalId=1189918454225211397, articleId=1200070544801493160, doi=null, pmid=null, pmcid=null, year=2010, volume=37, issue=5, pageStart=1209, pageEnd=1212, url=null, language=null, rfNumber=[12], rfOrder=17, authorNames=WU S C, QIN S Q, WANG S S, journalName=China Laser, refType=null, unstructuredReference=WU S C, QIN S Q, WANG S S, et al. A New Method for the Estimate of z-Direction Ring Laser Gyro Drift Based on Attitude Determination[J]. China Laser, 2010, 37(5): 1209-1212., articleTitle=A New Method for the Estimate of z-Direction Ring Laser Gyro Drift Based on Attitude Determination, refAbstract=null)], funds=null, companyList=[AuthorCompany(id=1200070548228239800, tenantId=1146029695717560320, journalId=1189918454225211397, articleId=1200070544801493160, xref=null, ext=[AuthorCompanyExt(id=1200070548240822715, tenantId=1146029695717560320, journalId=1189918454225211397, articleId=1200070544801493160, companyId=1200070548228239800, language=EN, country=null, province=null, city=null, postcode=null, companyName=null, departmentName=null, remark= Chongqing Jiaotong University, 400074), AuthorCompanyExt(id=1200070548312125892, tenantId=1146029695717560320, journalId=1189918454225211397, articleId=1200070544801493160, companyId=1200070548228239800, language=CN, country=null, province=null, city=null, postcode=null, companyName=null, departmentName=null, remark=重庆交通大学, 重庆 400074)])], figs=[ArticleFig(id=1200070549939516028, tenantId=1146029695717560320, journalId=1189918454225211397, articleId=1200070544801493160, language=EN, label=null, caption=null, figureFileSmall=DVkdUiLEyC+9lq2VyemAdw==, figureFileBig=whXpFaDUBCuoIU1+Uw3s7w==, tableContent=null), ArticleFig(id=1200070550044373639, tenantId=1146029695717560320, journalId=1189918454225211397, articleId=1200070544801493160, language=CN, label=图1, caption=多传感器融合定位系统框架, figureFileSmall=DVkdUiLEyC+9lq2VyemAdw==, figureFileBig=whXpFaDUBCuoIU1+Uw3s7w==, tableContent=null), ArticleFig(id=1200070550354752171, tenantId=1146029695717560320, journalId=1189918454225211397, articleId=1200070544801493160, language=EN, label=null, caption=null, figureFileSmall=QjzpElkVLr9Aw/9T6S8pDg==, figureFileBig=xL0SHJTPg17b1vuleiwZkg==, tableContent=null), ArticleFig(id=1200070550509941427, tenantId=1146029695717560320, journalId=1189918454225211397, articleId=1200070544801493160, language=CN, label=图2, caption=传感器输出对比, figureFileSmall=QjzpElkVLr9Aw/9T6S8pDg==, figureFileBig=xL0SHJTPg17b1vuleiwZkg==, tableContent=null), ArticleFig(id=1200070550644159171, tenantId=1146029695717560320, journalId=1189918454225211397, articleId=1200070544801493160, language=EN, label=null, caption=null, figureFileSmall=IuWfMkicWROuV/VcZmGYhg==, figureFileBig=0N+WNOSY3h0b+HftaWIk6A==, tableContent=null), ArticleFig(id=1200070550845485777, tenantId=1146029695717560320, journalId=1189918454225211397, articleId=1200070544801493160, language=CN, label=图3, caption=试验用智能车, figureFileSmall=IuWfMkicWROuV/VcZmGYhg==, figureFileBig=0N+WNOSY3h0b+HftaWIk6A==, tableContent=null), ArticleFig(id=1200070550975509214, tenantId=1146029695717560320, journalId=1189918454225211397, articleId=1200070544801493160, language=EN, label=null, caption=null, figureFileSmall=RQ0lYpF0y5Q2TXhadLoTlg==, figureFileBig=rpMgbOreliHAdhPlF//vzw==, tableContent=null), ArticleFig(id=1200070551113921260, tenantId=1146029695717560320, journalId=1189918454225211397, articleId=1200070544801493160, language=CN, label=图4, caption=数据采集路线, figureFileSmall=RQ0lYpF0y5Q2TXhadLoTlg==, figureFileBig=rpMgbOreliHAdhPlF//vzw==, tableContent=null), ArticleFig(id=1200070551252333304, tenantId=1146029695717560320, journalId=1189918454225211397, articleId=1200070544801493160, language=EN, label=null, caption=null, figureFileSmall=EVugmwVaZb/ZNAHfyRTiLw==, figureFileBig=S8ykNATan6YH3x9+hoX63A==, tableContent=null), ArticleFig(id=1200070551357190911, tenantId=1146029695717560320, journalId=1189918454225211397, articleId=1200070544801493160, language=CN, label=图5, caption=先验地图, figureFileSmall=EVugmwVaZb/ZNAHfyRTiLw==, figureFileBig=S8ykNATan6YH3x9+hoX63A==, tableContent=null), ArticleFig(id=1200070551478825742, tenantId=1146029695717560320, journalId=1189918454225211397, articleId=1200070544801493160, language=EN, label=null, caption=null, figureFileSmall=Um7SI0Ag8lWYM3nh28uYvg==, figureFileBig=VyGzUsQjIKPjp+9nGxk9Cw==, tableContent=null), ArticleFig(id=1200070551642403613, tenantId=1146029695717560320, journalId=1189918454225211397, articleId=1200070544801493160, language=CN, label=图6, caption=IMU航位推算轨迹、速度和位姿, figureFileSmall=Um7SI0Ag8lWYM3nh28uYvg==, figureFileBig=VyGzUsQjIKPjp+9nGxk9Cw==, tableContent=null), ArticleFig(id=1200070551759844136, tenantId=1146029695717560320, journalId=1189918454225211397, articleId=1200070544801493160, language=EN, label=null, caption=null, figureFileSmall=zGettv/70O6JuwQI3K/MtA==, figureFileBig=9vR/vHyE/S3rDITQpUmppQ==, tableContent=null), ArticleFig(id=1200070551868896049, tenantId=1146029695717560320, journalId=1189918454225211397, articleId=1200070544801493160, language=CN, label=图7, caption=轨迹对比, figureFileSmall=zGettv/70O6JuwQI3K/MtA==, figureFileBig=9vR/vHyE/S3rDITQpUmppQ==, tableContent=null), ArticleFig(id=1200070551982142270, tenantId=1146029695717560320, journalId=1189918454225211397, articleId=1200070544801493160, language=EN, label=null, caption=null, figureFileSmall=x/N0mc1OFyBNuJR2jMpcWA==, figureFileBig=hD8EJVcS9DYJEOIqhhaZOg==, tableContent=null), ArticleFig(id=1200070552061834056, tenantId=1146029695717560320, journalId=1189918454225211397, articleId=1200070544801493160, language=CN, label=图8, caption=特殊路段场景, figureFileSmall=x/N0mc1OFyBNuJR2jMpcWA==, figureFileBig=hD8EJVcS9DYJEOIqhhaZOg==, tableContent=null), ArticleFig(id=1200070552200246104, tenantId=1146029695717560320, journalId=1189918454225211397, articleId=1200070544801493160, language=EN, label=null, caption=null, figureFileSmall=otgYCf6ya/VQ6LjvvVIKvw==, figureFileBig=MXzJ7ppKJt7/vC+BAYOuuQ==, tableContent=null), ArticleFig(id=1200070552330269543, tenantId=1146029695717560320, journalId=1189918454225211397, articleId=1200070544801493160, language=CN, label=图9, caption=GNSS状态, figureFileSmall=otgYCf6ya/VQ6LjvvVIKvw==, figureFileBig=MXzJ7ppKJt7/vC+BAYOuuQ==, tableContent=null), ArticleFig(id=1200070552519013242, tenantId=1146029695717560320, journalId=1189918454225211397, articleId=1200070544801493160, language=EN, label=null, caption=null, figureFileSmall=null, figureFileBig=null, tableContent=
状态值 状态 状态值 状态
0 不定位不定向 3 组合推算
1 单点定位定向 4 实时动态(RTK)稳定解定位定向
2 伪距差分定位定向 5 RTK浮点解定位定向
), ArticleFig(id=1200070552665813892, tenantId=1146029695717560320, journalId=1189918454225211397, articleId=1200070544801493160, language=CN, label=表1, caption=

GNSS状态定义

, figureFileSmall=null, figureFileBig=null, tableContent=
状态值 状态 状态值 状态
0 不定位不定向 3 组合推算
1 单点定位定向 4 实时动态(RTK)稳定解定位定向
2 伪距差分定位定向 5 RTK浮点解定位定向
)], attaches=null, journal=Journal(id=1189918244568731652, delFlag=0, nameCn=汽车工程师, nameEn=Automotive Engineer, nameHistory1=null, nameHistory2=null, issn=1674-6546, eissn=null, cn=22-1432/U, coden=null, periodic=0, language=CN, oaType=null, ccby=null, superviseOffice=null, ownerOffice=null, pubOffice=null, editorOffice=null, officeType=null, aims=null, clcCode=null, officeProv=null, officeCity=null, officeAddr=null, officeZip=null, officeEmail=null, officePhone=null, editDirector=null, officeDirector=null, officeDirectorPhone=null, officeStaffNum=null, officeEmpNum=null, coverPicUrl=+bJsKkKt/pjz9u6EwhnksQ==, journalPrice=null, startedYear=null, abbrevIsoEn=null, journalRemark=null, publicationField=null, createdTime=1761628217121, updatedTime=1761735708780, createdBy=18614031015, updatedBy=13701087609, firstLetterCn=A, firstLetterEn=A, subjectCode=Engineering, subjectName=Engineering, subjectCodeEn=Engineering, subjectNameEn=null, picCn=+bJsKkKt/pjz9u6EwhnksQ==, picEn=O3Sn3tnYYrh/jm6emnnMWA==, jcr=null, cjcr=null, exts=[JournalExt(id=1190369097415233706, language=CN, name=汽车工程师, nameHistory1=null, nameHistory2=null, managedBy=, sponsoredBy=, publishedBy=, editorOffice=, officeProv=null, officeCity=null, officeAddr=, officeZip=, editDirector=, officeDirector=null, officePhone=null, coverPicUrl=null, journalRemark=, submitArticleUrl=null, websiteUrl=, createdTime=1761735708812, updatedTime=1761735708812, createdBy=13701087609, updatedBy=13701087609, submissionGuidelinesUrl=, submissionAuthorUrl=https://tjqc.cbpt.cnki.net/index.aspx?t=1, submissionEditorUrl=https://tjqc.cbpt.cnki.net/index.aspx?t=3, submissionReviewUrl=https://tjqc.cbpt.cnki.net/index.aspx?t=2, submissionCeEditorUrl=, submissionAeEditorUrl=, option={"copyright":""}), JournalExt(id=1190369097553645739, language=EN, name=Automotive Engineer, nameHistory1=null, nameHistory2=null, managedBy=, sponsoredBy=, publishedBy=, editorOffice=, officeProv=null, officeCity=null, officeAddr=, officeZip=, editDirector=, officeDirector=null, officePhone=null, coverPicUrl=null, journalRemark=, submitArticleUrl=null, websiteUrl=, createdTime=1761735708845, updatedTime=1761735708845, createdBy=13701087609, updatedBy=13701087609, submissionGuidelinesUrl=, submissionAuthorUrl=https://tjqc.cbpt.cnki.net/index.aspx?t=1, submissionEditorUrl=https://tjqc.cbpt.cnki.net/index.aspx?t=3, submissionReviewUrl=https://tjqc.cbpt.cnki.net/index.aspx?t=2, submissionCeEditorUrl=, submissionAeEditorUrl=, option={"copyright":""})], databaseList=null, tenantJournalId=1189918454225211397, websiteList=[Website(id=1189918982430847716, webName=null, webTitle=null, webDomain=null, webCopyrigh=null, webIpcNo=null, seoTitle=null, seoKeywords=null, seoDescription=null, tenantJournalId=null, journalId=1189918454225211397, journalNameCn=null, journalNameEn=null, grayFlag=null, tenantId=1146029695717560320, platformId=null, journalGroupId=null, journalGroupNameCn=null, journalGroupNameEn=null, type=1, domain=https://castjournals.cast.org.cn/joweb/qcgcs/CN, language=CN, createTime=1761628393037, createBy=18614031015, updateTime=1761628422913, updateBy=18614031015, name=汽车工程师-中文, tplId=1146099689490845704, title=汽车工程师, delFlag=0, indexPage=/home, props=[WebsiteProps(id=1189919800185917791, tenantId=1146029695717560320, journalId=null, journalGroupId=null, siteId=1189918982430847716, code=articleTextType, value=kx, createTime=1761628588005, updateTime=1761628588005, creator=18614031015, updator=18614031015), WebsiteProps(id=1189919800164946268, tenantId=1146029695717560320, journalId=null, journalGroupId=null, siteId=1189918982430847716, code=banner, value=null, createTime=1761628588000, updateTime=1761628588000, creator=18614031015, updator=18614031015), WebsiteProps(id=1189919800211083618, tenantId=1146029695717560320, journalId=null, journalGroupId=null, siteId=1189918982430847716, code=grayFlag, value=0, createTime=1761628588011, updateTime=1761628588011, creator=18614031015, updator=18614031015), WebsiteProps(id=1189919800156557659, tenantId=1146029695717560320, journalId=null, journalGroupId=null, siteId=1189918982430847716, code=logo, value=https://castjournals.cast.org.cn/joweb/qcgcs/CN/file/pic?fileId=yiZ96RYoYcnGnRMuWdmkWA==, createTime=1761628587998, updateTime=1761628587998, creator=18614031015, updator=18614031015), WebsiteProps(id=1189919800223666532, tenantId=1146029695717560320, journalId=null, journalGroupId=null, siteId=1189918982430847716, code=minRunFlag, value=0, createTime=1761628588014, updateTime=1761628588014, creator=18614031015, updator=18614031015), WebsiteProps(id=1189919800181723486, tenantId=1146029695717560320, journalId=null, journalGroupId=null, siteId=1189918982430847716, code=picServerUrl, value=https://castjournals.cast.org.cn/joweb/qcgcs/CN/file/pic, createTime=1761628588004, updateTime=1761628588004, creator=18614031015, updator=18614031015), WebsiteProps(id=1189919800215277923, tenantId=1146029695717560320, journalId=null, journalGroupId=null, siteId=1189918982430847716, code=silenceFlag, value=0, createTime=1761628588012, updateTime=1761628588012, creator=18614031015, updator=18614031015), WebsiteProps(id=1189919800173334877, tenantId=1146029695717560320, journalId=null, journalGroupId=null, siteId=1189918982430847716, code=staticResourcePath, value=https://castjournals.cast.org.cn/joweb/cast_kjdb_cn_619/, createTime=1761628588002, updateTime=1761628588002, creator=18614031015, updator=18614031015), WebsiteProps(id=1189919800194306400, tenantId=1146029695717560320, journalId=null, journalGroupId=null, siteId=1189918982430847716, code=themeColor, value=null, createTime=1761628588007, updateTime=1761628588007, creator=18614031015, updator=18614031015), WebsiteProps(id=1189919800202695009, tenantId=1146029695717560320, journalId=null, journalGroupId=null, siteId=1189918982430847716, code=themeStyle, value=null, createTime=1761628588009, updateTime=1761628588009, creator=18614031015, updator=18614031015)]), Website(id=1189918982527316711, webName=null, webTitle=null, webDomain=null, webCopyrigh=null, webIpcNo=null, seoTitle=null, seoKeywords=null, seoDescription=null, tenantJournalId=null, journalId=1189918454225211397, journalNameCn=null, journalNameEn=null, grayFlag=null, tenantId=1146029695717560320, platformId=null, journalGroupId=null, journalGroupNameCn=null, journalGroupNameEn=null, type=1, domain=https://castjournals.cast.org.cn/joweb/qcgcs/EN, language=EN, createTime=1761628393061, createBy=18614031015, updateTime=1761628543075, updateBy=18614031015, name=汽车工程师-英文, tplId=1146101810881728533, title=Automotive Engineer, delFlag=0, indexPage=/home, props=[WebsiteProps(id=1189919837561352952, tenantId=1146029695717560320, journalId=null, journalGroupId=null, siteId=1189918982527316711, code=articleTextType, value=kx, createTime=1761628596916, updateTime=1761628596916, creator=18614031015, updator=18614031015), WebsiteProps(id=1189919837540381429, tenantId=1146029695717560320, journalId=null, journalGroupId=null, siteId=1189918982527316711, code=banner, value=null, createTime=1761628596911, updateTime=1761628596911, creator=18614031015, updator=18614031015), WebsiteProps(id=1189919837582324475, tenantId=1146029695717560320, journalId=null, journalGroupId=null, siteId=1189918982527316711, code=grayFlag, value=0, createTime=1761628596921, updateTime=1761628596921, creator=18614031015, updator=18614031015), WebsiteProps(id=1189919837527798516, tenantId=1146029695717560320, journalId=null, journalGroupId=null, siteId=1189918982527316711, code=logo, value=https://castjournals.cast.org.cn/joweb/qcgcs/EN/file/pic?fileId=yiZ96RYoYcnGnRMuWdmkWA==, createTime=1761628596908, updateTime=1761628596908, creator=18614031015, updator=18614031015), WebsiteProps(id=1189919837594907389, tenantId=1146029695717560320, journalId=null, journalGroupId=null, siteId=1189918982527316711, code=minRunFlag, value=0, createTime=1761628596924, updateTime=1761628596924, creator=18614031015, updator=18614031015), WebsiteProps(id=1189919837557158647, tenantId=1146029695717560320, journalId=null, journalGroupId=null, siteId=1189918982527316711, code=picServerUrl, value=https://castjournals.cast.org.cn/joweb/qcgcs/EN/file/pic, createTime=1761628596915, updateTime=1761628596915, creator=18614031015, updator=18614031015), WebsiteProps(id=1189919837586518780, tenantId=1146029695717560320, journalId=null, journalGroupId=null, siteId=1189918982527316711, code=silenceFlag, value=0, createTime=1761628596922, updateTime=1761628596922, creator=18614031015, updator=18614031015), WebsiteProps(id=1189919837548770038, tenantId=1146029695717560320, journalId=null, journalGroupId=null, siteId=1189918982527316711, code=staticResourcePath, value=https://castjournals.cast.org.cn/joweb/cast_kjdb_en_623/, createTime=1761628596913, updateTime=1761628596913, creator=18614031015, updator=18614031015), WebsiteProps(id=1189919837569741561, tenantId=1146029695717560320, journalId=null, journalGroupId=null, siteId=1189918982527316711, code=themeColor, value=null, createTime=1761628596918, updateTime=1761628596918, creator=18614031015, updator=18614031015), WebsiteProps(id=1189919837573935866, tenantId=1146029695717560320, journalId=null, journalGroupId=null, siteId=1189918982527316711, code=themeStyle, value=null, createTime=1761628596919, updateTime=1761628596919, creator=18614031015, updator=18614031015)])], journalTitle=汽车工程师, weixinUrl=null, journalUrl=https://tjqc.cbpt.cnki.net/, iacademicId=null, status=1, seqNo=null, journalTitleEn=Automotive Engineer, journalPhotoCn=+bJsKkKt/pjz9u6EwhnksQ==, journalPhotoEn=O3Sn3tnYYrh/jm6emnnMWA==, journalFirstLetter=A, journalRecommend=null, journalNew=null, journalCollection=null, jcrJf=null, cjcrJf=null, jcrJfStr=null, cjcrJfStr=null, submissionFirstDecision=null, sciSubjectClassification=null, casSubjectClassification=null, citeScore=null, totalCitationFrequency=null, icpCode=null, psCode=null, advertisingLicenseCode=null, copyrightInformation=null, country=null, option=, provinceCode=null, provinceName=null, collectFlag=false), detailUrlCn=https://castjournals.cast.org.cn/joweb/qcgcs/CN/10.20104/j.cnki.1674-6546.20230462, detailUrlEn=https://castjournals.cast.org.cn/joweb/qcgcs/EN/10.20104/j.cnki.1674-6546.20230462, pdfUrlCn=https://castjournals.cast.org.cn/joweb/qcgcs/CN/PDF/10.20104/j.cnki.1674-6546.20230462, pdfUrlEn=https://castjournals.cast.org.cn/joweb/qcgcs/EN/PDF/10.20104/j.cnki.1674-6546.20230462, aliStartDate=null, aliEndDate=null, collectionFlag=false, citedCount=null, citedUrl=null, reference=null)
收藏切换
基于先验地图/GNSS/IMU融合的自动驾驶车辆定位系统
收藏切换
PDF下载
熊华川
汽车工程师 | 重庆交通大学自动驾驶环境感知与定位技术专题 2024,(1): 19-24
收起
收藏切换
汽车工程师 | 重庆交通大学自动驾驶环境感知与定位技术专题 2024, (1): 19-24
基于先验地图/GNSS/IMU融合的自动驾驶车辆定位系统
全屏
熊华川
作者信息
  • 重庆交通大学, 重庆 400074
Autonomous Vehicle Location Based on Prior Map/GNSS/IMU Fusion
Huachuan Xiong
Affiliations
  • Chongqing Jiaotong University, 400074
出版时间: 2024-01-15 doi: 10.20104/j.cnki.1674-6546.20230462
文章导航
收藏切换

针对仅依靠单一的传感器无法提供持续稳定的位置信息的问题,利用误差卡尔曼滤波(ESKF)算法,融合全球卫星导航系统(GNSS)、惯性测量单元(IMU)和先验地图点云数据,构建了高精度定位系统,并通过引入矩阵校正方法,避免GNSS失效或先验地图匹配延迟等情况下系统无法获取有效观测值以修正位姿的问题,使系统保持稳定并处于高精度状态。试验结果表明:在观测值失效等情况下,该方法仍然能够保持高精度的定位状态;相较于传统的单一传感器定位方法以及常见的GNSS/IMU组合定位方法,所提出的方法具有更高的鲁棒性和可靠性,能够更好地满足自动驾驶汽车在复杂环境中的定位需求。

先验地图  /  高精定位  /  多传感器融合  /  误差卡尔曼滤波

To solve the problem that a single sensor cannot provide continuous and stable position information. In this paper, an innovative method is proposed to build a high-precision positioning system by integrating three kinds of data, namely global satellite navigation system, Inertial Measurement Unit (IMU) and prior map point cloud matching, through the Error State Kalman Filter (ESKF) algorithm. At the same time, by introducing the correction matrix method, the pose problem can be corrected to avoid the system failing to obtain effective observations in the case of Global Navigation Satellite System (GNSS) failure or prior map matching delay, etc., so that the system can maintain stability and be in a high-precision state. The experimental results show that the proposed method can still maintain high precision in the case of failure of the observed value. Compared with the traditional single sensor positioning method and the common GNSS/IMU combined positioning method, the proposed method has higher robustness and reliability, and can better meet the positioning requirements of autonomous vehicles in complex environments.

Priori map  /  HD positioning  /  Multi-sensor fusion  /  Error State Kalman Filter (ESKF)
熊华川. 基于先验地图/GNSS/IMU融合的自动驾驶车辆定位系统. 汽车工程师, 2024 , (1) : 19 -24 . DOI: 10.20104/j.cnki.1674-6546.20230462
Huachuan Xiong. Autonomous Vehicle Location Based on Prior Map/GNSS/IMU Fusion[J]. Automotive Engineer, 2024 , (1) : 19 -24 . DOI: 10.20104/j.cnki.1674-6546.20230462
精确的定位是自动驾驶车辆实现自主导航的前提[1]。仅依靠单一传感器的传统定位方式无法克服复杂的环境条件,如全球卫星导航系统(Global Navigation Satellite System,GNSS)在隧道、建筑遮挡条件下极易失效,不能满足定位的稳定性需求。针对复杂多变的环境,将多个传感器的数据进行融合实现定位是目前主流的定位方式。
其中,GNSS/惯性测量单元(Inertial Measurement Unit,IMU)融合定位是目前较流行的定位方案[2]。李庆成等[3]利用IMU进行位姿、速度等状态预测,再利用扩展卡尔曼滤波(Extended Kalman Filter,EKF)算法融合GNSS和车速传感器数据来修正预测状态值,从而为矿用车提供精准的定位数据。Musavi等[4]同样通过融合GNSS和IMU数据实现精确定位,不同的是采用的融合算法为自适应神经观测器。李鹏等[5]在融合GNSS和IMU数据的基础上,提出丢弃恶化的观测量策略,以降低观测量精度较差时系统所受影响。因此,GNSS与IMU融合的定位算法均利用IMU通过航位推算技术进行位姿估计,IMU长时间积分会产生累积误差[6],需通过GNSS修正预测值。但当GNSS失效或者受环境影响变得低效时,反而会降低定位系统的精度。
目前,利用激光雷达点云与先验地图匹配定位的方法同样是研究热点。可以通过实时定位与地图构建(Simultaneous Localization and Mapping, SLAM)方法来构建先验地图,再利用激光雷达扫描得到的点云与先验地图进行匹配获取位姿。基于迭代最近点匹配(Iterative Closest Point,ICP)算法是其中最常见的方法[7-8],但该方法适用于少量点云的匹配运算。Droeschel等[9]提出通过构建栅格局部地图与点云进行匹配,并通过分层优化的方式优化匹配结果。以上方法都是在点云特征的基础上进行计算优化,当出现大规模点云,以及两帧之间数据变化剧烈等情况时,点云配准均出现耗时长、准确率低的现象。因此,仅依靠激光雷达的地图匹配方法不能满足定位系统的要求。
针对以上问题,本文提出利用误差卡尔曼滤波(Error State Kalman Filter,ESKF)算法融合GNSS、IMU和先验地图点云数据,再通过引入矩阵校正方法,避免GNSS失效或先验地图匹配延迟等情况下系统无法获取有效观测值以修正位姿的问题,使系统保持稳定和高精度状态。
本文算法的整体框架如图1所示。本文采用的数据融合算法为ESKF算法[10],相较于EKF算法,其具有以下优势[11]:ESKF主要针对误差状态量进行处理,误差状态量通常较小,因此计算量小,且总是在原点附近,可避免奇异值,保证线性化的合理性和有效性;在旋转处理方面,ESKF可以用三维变量来表示旋转增量。
本文的整体框架分为3个部分:
a. 数据预处理。接收到IMU原始数据和激光雷达数据后,根据IMU推算出车辆在激光雷达帧内的位姿变化,进而完成点云去畸变。
b. 激光与点云地图匹配。通过SLAM算法构建先验地图,并将GNSS信息作为先验地图的初始坐标,完成对多传感器输出位姿的坐标系统一,再利用正态分布变换(Normal-Distributions Transform,NDT)算法将每一帧去畸变的点云与先验地图进行匹配输出位姿。
c. 滤波。利用ESKF算法融合IMU推算出的状态值、激光雷达与先验地图匹配输出的位姿以及GNSS输出的位姿。通过观测信息修正IMU推算出的状态值。
本文所使用的IMU由陀螺仪和加速度计组成。根据二者特性可以对IMU测量的加速度和角速度进行数学建模:
$\left\{\begin{array}{l}{a}_{m}={C}_{NI}^{T}({a}_{t}-g)+{b}_{a}+{\eta }_{a}\\ {w}_{m}={w}_{t}+{b}_{w}+{\eta }_{w}\end{array}\right.$
式中,amwm分别为IMU加速度、角速度的测量值;atwt分别为IMU加速度、角速度的真实值;babw分别为加速度零偏和角速度零偏;ηaηw分别为加速度、角速度测量噪声;CNI为IMU坐标系到惯性坐标系的变换矩阵;g为重力加速度。
在初始化阶段,对IMU的加速度零偏和角速度零偏进行估计。具体而言,当车辆静止时,除加速度计的Z向加速度与重力加速度相等外,IMU的数值均应为零,因此车辆静止时IMU的加速度和角速度测量结果即可认为是IMU的零偏值[12]。随后,使用激光雷达匹配先验地图输出的位姿作为基准,对初始位姿进行估计。这种方法使得系统能够在任何地点进行初始化,无需考虑GNSS的状态。
ESKF将系统状态分为不考虑噪声的名义状态和误差状态。二者相加即为真值状态:
Xt=XnX
其中:
${X}_{n}={\left[\begin{array}{cccc}{p}^{T}& {v}^{T}& {q}^{T}& \begin{array}{cc}{b}_{a}^{T}& \begin{array}{cc}{b}_{w}^{T}& {g}^{T}\end{array}\end{array}\end{array}\right]}^{T}$
$\delta X={\left[\begin{array}{cccc}\delta {p}^{T}& \delta {v}^{T}& \delta {\theta }^{T}& \begin{array}{cc}\delta {b}_{a}^{T}& \begin{array}{cc}\delta {b}_{w}^{T}& \delta {g}^{T}\end{array}\end{array}\end{array}\right]}^{T}$
式中,Xt为系统真值状态;Xn为系统名义状态;δX为系统误差状态;p、δp分别为名义位置状态量、误差位置状态量;v、δv分别为名义速度状态量、误差速度状态量;q、δθ分别为名义姿态状态量、角度误差状态量;δg为误差重力加速度状态量;δbw、δba分别为误差角速度零偏状态量、误差加速度零偏状态量。
根据IMU航位推算算法,可以得到名义状态的离散形式:
$\left\{\begin{array}{l}{p}_{k+1}={p}_{k}+v\Delta t+\frac{1}{2}\left({C}_{NI}\right({a}_{m}-{b}_{a})+g)\Delta {t}^{2}\\ {v}_{k+1}={v}_{k}+\left({C}_{NI}\right({a}_{m}-{b}_{a})+g)\Delta t\\ {q}_{k+1}={q}_{k}\otimes q\left\{\right({w}_{m}-{b}_{w}\left)\Delta t\right\}\\ {b}_{a(k+\left.1\right)}={b}_{a\left(k\right)}\\ {b}_{w(k+\left.1\right)}={b}_{w\left(k\right)}\\ {g}_{k+1}={g}_{k}\end{array}\right.$
式中,pkvkqkba(k)bw(k)gk分别为k时刻名义位置状态量、名义速度状态量、名义姿态状态量、加速度零偏状态量、角速度零偏状态量、重力加速度状态量。
根据名义状态与误差状态的关系可以推算出误差状态的离散形式:
$\left\{\begin{array}{l}\delta {p}_{k+1}=\delta {p}_{k}+v\Delta t\\ \delta {v}_{k+1}=\delta {v}_{k}+\left({C}_{NI}\right({a}_{m}-{b}_{a})+g)\Delta t+{\eta }_{\nu }\\ \delta {\theta }_{k+1}={C}_{NI}^{T}\left\{\right({w}_{m}-{b}_{w}\left)\Delta t\right\}\delta {\theta }_{k}+{\eta }_{\theta }\\ \delta {b}_{a(k+\left.1\right)}=\delta {b}_{a\left(k\right)}+{\eta }_{ba}\\ \delta {b}_{w(k+\left.1\right)}=\delta {b}_{w\left(k\right)}+{\eta }_{bw}\\ \delta {g}_{k+1}=\delta {g}_{k}\end{array}\right.$
式中,δpk、δvk、δθk、δba(k)、δbw(k)、δgk分别为k时刻误差位置状态量、误差速度状态量、角度误差状态量、误差加速度零偏状态量、误差角速度零偏状态量、误差重力加速度状态量;ηvηθηbaηbw为随机噪声;Δt为IMU采样时间。
根据系统误差状态方程,可以得到ESKF预测方程和预测的协方差矩阵:
$\delta {X}_{k+1}^{-}={J}_{X}\delta {X}_{k}^{+}+{J}_{i}i,i=\left[\begin{array}{c}{\eta }_{\nu }\\ {\eta }_{\theta }\\ {\eta }_{a}\\ {\eta }_{w}\end{array}\right]$
${P}_{k+1}^{-}={J}_{X}{P}_{k}^{+}{J}_{X}^{T}+{J}_{i}{Q}_{i}{J}_{i}^{T}$
其中:
${J}_{X}=\left[\begin{array}{cccc}\begin{array}{c}I\\ 0\\ 0\end{array}& \begin{array}{c}I\Delta t\\ I\\ 0\end{array}& \begin{array}{c}0\\ -{C}_{NI}{\left\{\stackrel{-}{a}\right\}}_{\times }\Delta t\\ {C}_{NI}^{T}\left\{\stackrel{-}{w}\Delta t\right\}\end{array}& \begin{array}{c}\begin{array}{cc}0& \begin{array}{cc}0& 0\end{array}\end{array}\\ \begin{array}{ccc}-C\Delta t& 0& I\Delta t\end{array}\\ \begin{array}{ccc}0& -I\Delta t& 0\end{array}\end{array}\\ 0& 0& 0& \begin{array}{ccc}I& 0& 0\end{array}\\ 0& 0& 0& \begin{array}{ccc}0& I& 0\end{array}\\ 0& 0& 0& \begin{array}{ccc}0& 0& I\end{array}\end{array}\right]$
${J}_{i}=\left[\begin{array}{c}\begin{array}{cc}0& \begin{array}{cc}\begin{array}{cc}0& 0\end{array}& 0\end{array}\end{array}\\ \begin{array}{cc}I& \begin{array}{cc}\begin{array}{cc}0& 0\end{array}& 0\end{array}\end{array}\\ \begin{array}{cc}0& \begin{array}{cc}\begin{array}{cc}I& 0\end{array}& 0\end{array}\end{array}\\ \begin{array}{c}\begin{array}{c}\begin{array}{cc}0& \begin{array}{cc}\begin{array}{cc}0& I\end{array}& 0\end{array}\end{array}\\ \begin{array}{cc}0& \begin{array}{cc}\begin{array}{cc}0& 0\end{array}& I\end{array}\end{array}\end{array}\\ \begin{array}{cc}0& \begin{array}{cc}\begin{array}{cc}0& 0\end{array}& 0\end{array}\end{array}\end{array}\end{array}\right]$
$\stackrel{-}{w}={w}_{m}-{b}_{w}$
$\stackrel{-}{a}={a}_{m}-{b}_{a}$
式中,$\mathrm{ }{\mathrm{\delta }\mathit{X}}_{k}^{+}$${\mathrm{\delta }\mathit{X}}_{k+1}^{-}$分别为k时刻后验误差状态量、(k+1)时刻先验误差状态量;JXJi分别为线性化后的雅可比矩阵;$\mathrm{ }{\mathit{P}}_{k}^{+}$${\mathit{P}}_{k+1}^{-}$分别为k时刻后验协方差矩阵、(k+1)时刻先验协方差矩阵;Qi为噪声协方差矩阵;I为单位阵;{ }×表示反对称矩阵。
本文的观测数据分别由GNSS和激光雷达与先验地图提供。二者的观测模型构建为:
$\left\{\begin{array}{l}{Y}_{GNSS}=h\left({X}_{t}\right)+V\\ {Y}_{LiDAR}=f\left({X}_{t}\right)+R\end{array}\right.$
式中,YGNSSYLiDAR为GNSS观测量、雷达匹配观测量;h( )、f( )分别为GNSS观测方程、雷达匹配观测方程;VR分别为GNSS观测量的协方差矩阵、雷达匹配观测量的协方差矩阵。
根据二者的输出为位姿,可以将观测模型统一写作:
$\left\{\begin{array}{l}Y=F{X}_{t}+{P}_{obs}\\ F=\left[\begin{array}{c}\begin{array}{cccc}\begin{array}{cc}\begin{array}{cc}I& 0\end{array}& 0\end{array}& 0& 0& 0\end{array}\\ \begin{array}{cccc}\begin{array}{cc}0& \begin{array}{cc}0& I\end{array}\end{array}& 0& 0& 0\end{array}\end{array}\right]\\ H=\frac{\partial F{X}_{t}}{\partial {X}_{t}}\frac{\partial {X}_{t}}{\partial \delta X}\end{array}\right.$
式中,Y为观测量;F为观测方程系数矩阵;Pobs为观测量的协方差矩阵;H为观测方程对误差状态的雅可比矩阵。
根据ESKF的修正公式,可以分别得到对误差状态的修正值和协方差矩阵:
$\left\{\begin{array}{l}K={P}_{k+1}^{-}{H}^{T}(H{P}_{k+1}^{-}{H}^{T}+{P}_{obs}{)}^{-1}\\ \delta {X}_{k+1}^{+}=K(Y-H{X}_{n(k+1)})\\ {P}_{k+1}^{+}=(I-KH){P}_{k+1}^{-}\\ {X}_{certify(k+1)}={X}_{n(k+1}\oplus \delta {X}_{k+1}^{+}\end{array}\right.$
式中,K为卡尔曼增益;Xcertify(k+1)为(k+1)时刻修正后的状态量;Xn(k+1)为(k+1)时刻名义状态量;⊕表示广义相加。
随后,通过将修正后的误差量与名义状态量进行广义相加即可得到最终的系统真值状态量。这一过程能够有效减小预测值的误差,提高系统的精度。
GNSS信号在高楼、隧道等环境下极易受到干扰,导致定位精度下降甚至失效,因此不能作为观测值使用。激光雷达虽然可以克服环境的影响,但其匹配过程耗时较长,无法满足车辆对实时定位的需求。另一方面,IMU的采样频率远高于GNSS和激光雷达的采样频率,不同频率下的观测值同样不能实时更新,如图2所示。针对以上问题,本文提出一种矩阵校正方法,以在GNSS失效或激光雷达匹配延迟的情况下近似估计系统的状态,避免系统因累积误差快速发散。具体地,利用修正位姿与预测位姿之间的相对位姿作为校正矩阵,当下一帧没有合适的观测值时,则利用校正矩阵修正系统位姿,以弥补GNSS和激光雷达的局限性,提高系统的实时性和定位精度:
$\Delta C={C}_{certify}^{T}{C}_{predict}$
式中,ΔC为校正矩阵;CcertifyCpredict分别为位姿的修正矩阵和预测位姿。
本文试验由基于吉利博瑞改装的智能车实现,如图3所示。智能车搭载的试验设备有:车顶前、后放置的2根GNSS蘑菇头天线,车顶中央处3颗16线激光雷达,后备箱处的GNSS信号接收机和IMU设备。
为验证本文算法的有效性,首先通过试验车在校园内进行GNSS、IMU和激光雷达数据采集,采集路线如图4所示,分别通过空旷路段、遮挡和隧道路段,来验证当GNSS失效或低效状态下系统状态的稳定性。
根据采集的激光雷达数据,通过SLAM算法构建定位系统中的先验地图,如图5所示。
为验证定位系统的有效性,根据采集的数据分别进行IMU航位推算、GNSS/IMU组合定位和先验地图/GNSS/IMU组合定位的算法验证以及结果对比。
图6所示为IMU进行航位推算的结果,可见IMU在积分时速度随着误差累积而发散,导致最终的轨迹和以四元数[qw,qx,qy,qz]T表示的位姿均随之发散,由式(5)可知,发散的速度成指数级增长,因此需要其他数据来抑制IMU的误差。
分别利用GNSS/IMU组合定位算法和先验地图/GNSS/IMU组合定位算法生成定位轨迹,如图7所示。其中包括树林遮挡、隧道等影响GNSS状态的路段,如图8所示。图9所示为整条轨迹中GNSS状态变化曲线,其中GNSS状态定义如表1所示。由图9可以看出,经过特殊路段时,GNSS的状态都不为4,根据表1,GNSS其处于精度较差甚至失效状态。从图7中可知,在空旷路段,两类算法输出的轨迹几乎重合,且平滑性好。差别体现在GNSS低效处,即图7中特殊路段处,GNSS/IMU组合算法明显较先验地图/GNSS/IMU组合算法具有更大的误差偏移。并且值得注意的是,当GNSS失效后,轨迹并没有立即如图6一样发散,而是随着GNSS失效后的时间推移而逐渐发散,这是校正矩阵作用的结果,校正矩阵可有效延缓系统状态的发散。
本文通过ESKF算法融合GNSS/IMU/先验地图构建自动驾驶车辆实时定位系统,并引入校正矩阵提高了系统的鲁棒性。试验结果表明,在GNSS低效甚至失效的情况下,先验地图/GNSS/IMU组合算法能够很好地抑制系统因误差累积而发散,从而为自动驾驶车辆提供稳定、精准的定位结果。
但是,本文提出的方法依赖于高精地图,高精地图的制作和维护将限制其应用。下一步的改进方向应是从高精点云地图中提取道路交通信息,以构建矢量地图。矢量地图的维护成本远小于高精点云地图的维护成本,且道路交通信息不会轻易改变。
参考文献 引证文献
排序方式:
[1]
OATES T. Book Review: Introduction to Autonomous Mobile Robots[J]. Artificial Intelligence, 2005, 169(2): 146-149.
[2]
朱洪达, 罗强, 辛琪. 基于多传感器融合的定位技术概述[J]. 南方农机, 2023, 54(18): 165-167.
ZHU H D, LUO Q, XIN Q. Overview of Positioning Technology Based on Multi-Sensor Fusion[J]. South Farm Machinery, 2023, 54(18): 165-167.
[3]
李庆成, 王玉林, 逯宇, 等. 基于IMU/GNSS/车速传感器的矿用车定位系统研究[J]. 青岛大学学报(工程技术版), 2023, 38(2): 75-81.
LI Q C, WANG Y L, LU Y, et al. Research on Positioning System of Mining Vehicle Based on IMU/GNSS/Speed Sensor[J]. Journal of Qingdao University (Engineering and Technology Edition), 2023, 38(2): 75-81.
[4]
MUSAVI N, KEIGHOBADI J. Adaptive Fuzzy Neuro-Observer Applied to Low Cost INS/GPS[J]. Applied Soft Computing Journal, 2015, 29: 82-94.
[5]
李鹏, 陆明泉, 冯振明. GNSS/IMU组合导航观测量丢弃策略[J]. 清华大学学报(自然科学版), 2011, 51(1): 122-125.
LI P, LU M Q, FENG Z M. Measurement Discarding Strategy for GNSS/IMU Integration[J]. Journal of Tsinghua University (Natural Science Edition), 2011, 51(1): 122-125.
[6]
缪玲娟, 李春明, 郭振西, 等. 陆用捷联惯导系统/里程计自主式组合导航技术[J]. 北京理工大学学报, 2004(9): 808-811.
MIAO L J, LI C M, GUO Z X, et al. Independently Integrated Navigation System of SINS and Distance-Transfer-Unit for Land Vehicles[J]. Transactions of Beijing Institute of Technology, 2004(9): 808-811.
[7]
MOOSMANN F, STILLER C. Velodyne SLAM[C]// 2011 IEEE Intelligent Vehicles Symposium (IV). Baden-Baden, Germany: IEEE, 2011.
[8]
YANG J L, LI H D, JIA Y D. Go-ICP: Solving 3D Registration Efficiently and Globally Optimally[C]// 2013 IEEE International Conference on Computer Vision. Sydney, NSW, Australia: IEEE, 2013.
[9]
DROESCHEL D, SCHWARZ M, BEHNKE S. Continuous Mapping and Localization for Autonomous Navigation in Rough Terrain Using a 3D Laser Scanner[J]. Robotics & Autonomous Systems, 2016, 88: 104-115.
[10]
高翔. 自动驾驶与机器人中的SLAM技术: 从理论到实践[M]. 北京. 电子工业出版社, 2023: 73-74.
GAO X. SLAM in Autonomous Driving and Robotics: From Theory to Practice[M]. Beijing: Electronics Industry Press, 2023: 73-74.
[11]
SOLÀ J. Quaternion Kinematics for the Error-State Kalman Filter[EB/OL]. (2017-11-03) [2023-10-29]. https://arxiv.org/abs/1711.02508 https://arxiv.org/abs/1711.02508
[12]
吴赛成, 秦石乔, 王省书, 等. 基于姿态解算的z向激光陀螺零偏估计方法[J]. 中国激光, 2010, 37(5): 1209-1212.
WU S C, QIN S Q, WANG S S, et al. A New Method for the Estimate of z-Direction Ring Laser Gyro Drift Based on Attitude Determination[J]. China Laser, 2010, 37(5): 1209-1212.
2024年第卷第1期
PDF下载
222
92
引用本文
BibTeX
文章信息
doi: 10.20104/j.cnki.1674-6546.20230462
  • 首发时间:2025-11-25
  • 出版时间:2024-01-15
补充材料
相关文章
文章信息
作者
出版历史
  • 修回日期:2023-10-29
基金
作者信息
    重庆交通大学, 重庆 400074
参考文献
分享链接
https://castjournals.cast.org.cn/joweb/qcgcs/CN/10.20104/j.cnki.1674-6546.20230462
分享至
全文二维码

扫描看全文

引用本文
BibTeX
本文的引用情况
2种不同金属材料的力学参数

Family
属数
Number of
genus
种数
Number of
species
占总种数比例
Percentage of
total species (%)

Genus
种数
Number of
species
占总种数比例
Percentage of total
species (%)
鹅膏菌科Amanitaceae 2 11 5.26 鹅膏菌属 Amanita 10 4.78
小菇科 Mycenaceae 2 12 5.74 丝盖伞属 Inocybe 5 2.39
多孔菌科 Polyporaceae 8 14 6.70 蜡蘑属 Laccaria 5 2.39
红菇科 Russulaceae 3 23 11.00 小皮伞属 Marasmius 6 2.87
小菇属 Mycena 11 5.26
光柄菇属 Pluteus 5 2.39
红菇属 Russula 17 8.13
栓菌属 Trametes 5 2.39
关闭全屏