CN117289294B - 一种基于多分辨率贝叶斯网格的融合定位方法 - Google Patents

一种基于多分辨率贝叶斯网格的融合定位方法 Download PDF

Info

Publication number
CN117289294B
CN117289294B CN202311585869.2A CN202311585869A CN117289294B CN 117289294 B CN117289294 B CN 117289294B CN 202311585869 A CN202311585869 A CN 202311585869A CN 117289294 B CN117289294 B CN 117289294B
Authority
CN
China
Prior art keywords
grid
probability
vehicle
pose
motion
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202311585869.2A
Other languages
English (en)
Other versions
CN117289294A (zh
Inventor
王维
黄浩哲
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Ruiyi Technology Shandong Co ltd
Original Assignee
Ruiyi Technology Shandong Co ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Ruiyi Technology Shandong Co ltd filed Critical Ruiyi Technology Shandong Co ltd
Priority to CN202311585869.2A priority Critical patent/CN117289294B/zh
Publication of CN117289294A publication Critical patent/CN117289294A/zh
Application granted granted Critical
Publication of CN117289294B publication Critical patent/CN117289294B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/86—Combinations of lidar systems with systems other than lidar, radar or sonar, e.g. with direction finders
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/28—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network with correlation of data from several navigational instruments
    • G01C21/30—Map- or contour-matching
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/02—Systems using the reflection of electromagnetic waves other than radio waves
    • G01S17/06—Systems determining position data of a target
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F18/00—Pattern recognition
    • G06F18/20—Analysing
    • G06F18/25—Fusion techniques
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06N—COMPUTING ARRANGEMENTS BASED ON SPECIFIC COMPUTATIONAL MODELS
    • G06N7/00—Computing arrangements based on specific mathematical models
    • G06N7/01—Probabilistic graphical models, e.g. probabilistic networks
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • General Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Electromagnetism (AREA)
  • Data Mining & Analysis (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Automation & Control Theory (AREA)
  • Artificial Intelligence (AREA)
  • General Engineering & Computer Science (AREA)
  • Evolutionary Computation (AREA)
  • Evolutionary Biology (AREA)
  • Computational Mathematics (AREA)
  • Bioinformatics & Computational Biology (AREA)
  • Bioinformatics & Cheminformatics (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Probability & Statistics with Applications (AREA)
  • Algebra (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Mathematical Analysis (AREA)
  • Mathematical Optimization (AREA)
  • Pure & Applied Mathematics (AREA)
  • Computing Systems (AREA)
  • Mathematical Physics (AREA)
  • Software Systems (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明属于自动驾驶相关技术领域，是一种基于多分辨率贝叶斯网格的融合定位方法，包括步骤：获取车辆里程计信息以及激光雷达数据；通过点云匹配得到车辆的初始位姿，在初始位姿附近构建一个贝叶斯全概率空间；从车辆里程计信息获取运动控制向量，与车辆初始位姿一起输入进行运动估计，得到一个预测的车辆位置；更新概率空间中栅格概率；分别计算栅格权重后与栅格概率结合形成最终概率，选中当前空间中概率最高的栅格，将其所代表的位姿信息作为最终结果输出。本方法以贝叶斯理论基础，通过贝叶斯滤波的方法搭建无人车定位框架，使用车载里程计作为状态输入，利用高精度点云匹配定位作为观测，最后输出最优定位。

Description

一种基于多分辨率贝叶斯网格的融合定位方法
技术领域
本发明主要涉及自动驾驶相关技术领域，具体是一种基于多分辨率贝叶斯网格的融合定位方法。
背景技术
在自动驾驶车辆行驶时，车辆需要做到厘米级的精准定位。例如，自动驾驶车辆需要估计其在地图上的精确位置及车与道路路沿的距离。自动驾驶系统通过点云配准技术，将实时采集到的点云数据与高精地图的数据做匹配，为自动驾驶车辆给出精确的定位。
自动驾驶技术近几年发展迅猛,这也促进了实时定位的发展。在特征丰富的环境中基于雷达的定位算法研究成果丰富,但在特征一致的退化环境（或弱特征环境）下,目前的算法点云会发生退化问题,定位精度较低,表现效果不佳，这限制了无人车在特殊环境下的自主行驶。
目前已有的定位方式也是百花齐放，种类繁多。对于室外来说,主流的就是采用全球卫星导航系统例如 GPS 获得经纬度海拔高进行全局定位，但是如果其上位机接收到的卫星数量很少,误差就会在 10 米以上,除此之外,其信息更新的频率过低。对于另一种主流的惯性导航定位来说,室内室外都可以使用，而且可以克服周围环境的不利因素，但是由于自身采用运动学积分会随时间推移产生越来越大的误差。这也促使了同步定位与建图(Simultaneous Localization andMapping, SLAM)技术的迅速发展,使之成为无人驾驶领域一大研究热点。它的本质就是无人车搭载感知外界信息的传感器,比如雷达,通过分析采集的丰富帧间特征信息的变化,来判断自身相对于起点的运动情况,实现实时定位,并能够同时生成描述刻画外界的点云地图。这个地图可以提供给后续的全局路径规划模块进行路径生成,同时也可以提供给其他车辆实现基于点云地图的匹配定位,是一个对无人车自动驾驶一举多得的系统。但是当车辆在无丰富建筑物标志的环境下行驶时,对无人车的实时定位提出了挑战。在 GPS 更新频率不足以满足实时性的前提下,依靠航迹推算的惯性定位又存在着无法消除的累计误差,和它们一样,基于雷达的 SLAM 系统在这种恶劣环境下一样会受到限制,会出现运动估计不精确甚至完全失效的退化问题,连带着生成的地图也是混乱的,无法准确地描绘世界环境,由于雷达本身的机械设备特点,只能采集外界三维特征,而现有 SLAM 系统又大多根据特征进行匹配定位,因此只依靠激光雷达很难解决这种问题。
现有技术中，如公开号为CN116222588B，名称为一种GPS与车载里程计融合的定位方法的相关技术中，通过粒子滤波的方法搭建多传感器融合框架，能够一定程度上为自动驾驶车提供连续、平滑、实时的高精度定位信息，然而粒子滤波在使用的过程中严重依赖于对初始状态的估计，可能很快收敛，也可能很快发散；有粒子退化问题，由于根据权重采样，又有粒子匮乏，丢失部分数据等问题。
发明内容
为解决目前技术的不足，本发明结合现有技术，从实际应用出发，提供一种基于多分辨率贝叶斯网格的融合定位方法，本方法是基于贝叶斯理论进行了弱特征环境下以雷达为主、组合导航设备为辅的定位研究，致力于实现一种弱特征条件下的融合定位方法。
本发明的技术方案如下：
一种基于多分辨率贝叶斯网格的融合定位方法，包括如下步骤：
S1、获取车辆里程计信息以及激光雷达数据；
S2、对激光雷达数据进行拼接以及特征提取，通过点云匹配得到车辆的初始位姿，在初始位姿附近构建一个贝叶斯全概率空间；
S3、从车辆里程计信息获取运动控制向量，与车辆初始位姿一起输入里程计运动模型进行运动估计，得到一个预测的车辆位置，同时，全概率空间随车辆位置变动转移到预测位置附近；
S4、依次计算上一次全概率空间中每个大栅格运动到当前全概率空间中单个大栅格的概率，再用同样的方式计算大栅格中的小栅格概率，将全概率空间运动后的每个小栅格中的概率进行统计后，按照小栅格所含概率大小重新分配小栅格对应概率；
S5、在重新分配概率后的贝叶斯全概率空间中引入当前的点云匹配定位信息作为观测值，计算每个小栅格的权重值后与该小栅格对应的概率结合，形成该小栅格最终的选择概率，比较整个全概率空间中各小栅格的选择概率，选出概率最高者作为本次最优位姿输出；
S6、重复执行步骤S3-S5，进行车辆位置及对应全概率空间的迭代求解以实现车辆的高精度定位。
进一步，步骤S1中，利用车辆中的CAN口与车载里程计进行数据交换以获取里程计信息，利用车辆的网口接收激光雷达数据，再通过交换机传输到车辆计算单元。
进一步，在初始位姿附近构建一个贝叶斯全概率空间，并根据精度需求调整该空间的大、小栅格分辨率，每个栅格中的概率均等，车辆在各方向上的位姿信息对应于栅格在全概率空间中的X、Y、Z轴上的坐标。
进一步，从车辆里程计信息获取运动控制向量，与车辆初始位姿一起输入到里程计运动模型中进行运动估计；
里程计运动模型表示车辆由初始位姿xt-1= [ X Y θ ]T运动至预测位姿xt= [ X ′Y ′ θ ′ ]T可拆分为车辆首先旋转角度δrot1，再沿着此方向平移δtrans，随后再次旋转角度δrot2抵达目标位姿，表达式如下：
；
上式，X表示初始位姿栅格在X轴上的坐标，Y表示初始位姿栅格在Y轴上的坐标，θ表示初始位姿栅格的角度，X ′表示预测位姿栅格在X轴上的坐标，Y ′表示预测位姿栅格在Y轴上的坐标，θ ′表示预测位姿栅格的角度。
进一步，依次计算上一次全概率空间中每个大栅格运动到当前全概率空间中单个大栅格的概率，具体包括：在里程计运动模型中加入全概率空间对应大栅格运动前的位姿、运动后的位姿和运动控制向量，大栅格运动前对应位姿，运动后对应位姿/>，输入到里程计运动模型得到：
；
表示大栅格运动时首先旋转的角度，/>大栅格旋转后沿此方向平移的距离，/>表示大栅格再次旋转的角度，/>表示运动前大栅格在X轴上的坐标，/>表示运动前大栅格在Y轴上的坐标，/>表示运动前大栅格的角度，/>表示运动后大栅格在X轴上的坐标，表示运动后大栅格在Y轴上的坐标，/>表示运动后大栅格的角度；
然后进行闭式计算得到大栅格运动前位置到达大栅格运动后位置的概率P，P计算方式如下：
；
其中，表示平移运动中平移分量的噪声，/>表示平移运动中旋转分量的噪声，表示旋转运动中平移分量的噪声，/>表示旋转运动中旋转分量的噪声。
进一步，每个小栅格的权重值计算方式如下：
；
上式中，为全概率空间运动后对应小栅格的位姿信息轴，为全概率空间运动 后对应小栅格的位姿信息轴，为全概率空间运动后对应小栅格后的位姿信息角度，、、为点云对应的位姿信息，，，为，，上对应的重要性系数， 为第个小栅格的权重。
进一步，选择概率计算方式如下：
重新分配概率后第个小栅格对应概率/>表示为：
；
其中，表示小栅格数量参数，将重新分配概率后第/>个小栅格对应概率/>与对应的权重/>结合形成第/>个小栅格对应的最终选择概率/>，
。
本发明的有益效果：
1、本发明根据高精度点云匹配以及里程计传感器输出信息,建立无人车点云地图定位模型,能够提高在弱特征环境下的定位精度,提升组合系统在各复杂场景下的可靠性与可用性；并在弱特征区域导致点云匹配信息短时间缺失的环境下,可以利用车载里程计信息可以对定位进行短时间约束而不发散,从而实现强抗干扰性的导航定位系统,提升了整个系统位置服务的性能。
2、本发明相比于粒子滤波的方法，贝叶斯网络定位容易用图形形式展示变量之间的依赖关系，对于人类用户和决策者来说，这种可视化方法更加易于理解和接受。此外，贝叶斯网络还可以使用简单的条件概率表格来表示依赖关系，这种表格对于各种人群都十分简单易懂。
3、在进行数据分析时，有时会出现缺失数据的情况，这些数据很可能是由于某种原因没有被记录下来，本发明贝叶斯网络可以利用其他数据的信息来补充缺失数据的不足，从而提高分析的准确性和可靠性。
4、本发明贝叶斯网络在处理大量变量之间的复杂关系时，能够有效地描述变量之间的依赖关系；可以通过对先验知识的引入，对概率分布进行修正；可以进行模型的推断和预测，对未来情况进行预测；可以逐步加入新的变量，构建动态的模型。
附图说明
图1为本发明的原理框图。
图2为里程计运动建模示意图。
具体实施方式
结合附图和具体实施例，对本发明作进一步说明。应理解，这些实施例仅用于说明本发明而不用于限制本发明的范围。此外应理解，在阅读了本发明讲授的内容之后，本领域技术人员可以对本发明作各种改动或修改，这些等价形式同样落于本申请所限定的范围。
本实施例提供一种基于多分辨率贝叶斯网络的融合定位方法，参考图1所示为本实施例的原理框图。
在本实施例中，为了克服现有技术固有的局限性，对现有技术存在的问题，提出了一种对车辆进行准确定位的用于自动驾驶场景的一种弱特征条件下的融合定位方法，为自动驾驶车提供连续、平滑、实时的高精度定位信息。
本实施例以贝叶斯理论基础，通过贝叶斯滤波的方法搭建无人车定位框架，使用车载里程计作为状态输入，利用高精度点云匹配定位作为观测，最后输出最优定位，本实施例的融合定位方法主要包括有如下步骤。
步骤S1、利用车辆中的CAN口通讯和网口，与各种传感器进行数据交互，主要通过CAN口与车载里程计进行数据交换以获取里程计信息，同时利用网线网口接收多个激光雷达数据，再通过交换机传输到车辆计算单元。
步骤S2、从网口获取到的多个激光雷达数据进行拼接及特征提取，再通过高精度点云匹配得到车辆的初始位姿，在初始位姿附近构建一个贝叶斯全概率空间，并根据精度需求调整该空间的大、小栅格分辨率，每个栅格中的概率均等，无人车在各方向上的位姿信息对应于栅格在全概率空间中的X、Y、Z轴上的坐标。
步骤S3、从里程计获取运动控制向量，与车辆初始位姿一起输入到里程计运动模型中进行运动估计，得到一个预测的车辆位置，同时，全概率空间随车辆位置变动整体运动到预测位置附近。
在本步骤中，首先对于里程计运动模型进行建模，参考图2所示，里程计运动模型中，车辆由初始位姿xt-1= [ X Y θ ]T运动至当前时刻位姿xt= [ X ′ Y ′ θ ′ ]T可拆分为车辆首先旋转角度δrot1，再沿着此方向平移δtrans，随后再次旋转角度δrot2抵达目标位姿，如此可以得到里程计运动模型的基本形式如下式（1）所示：
；
上式，X表示初始位姿栅格在X轴上的坐标，Y表示初始位姿栅格在Y轴上的坐标，θ表示初始位姿栅格的角度，X ′表示预测位姿栅格在X轴上的坐标，Y ′表示预测位姿栅格在Y轴上的坐标，θ ′表示预测位姿栅格的角度。
步骤S4、依次计算上一次全概率空间中每个大栅格运动到当前全概率空间中单个大栅格的概率（即在里程计运动模型中加入全概率空间对应大栅格运动前的位置、全概率空间对应大栅格运动后位置和运动控制向量，然后进行闭式计算得到大栅格运动前位置到达大栅格运动后位置的概率）；再用同样的方式计算大栅格中的小栅格概率，再将全概率空间运动后的每个小栅格中的概率进行统计后，按照小栅格所含概率大小重新分配小栅格对应概率。
在本步骤中，首先在里程计运动模型中加入全概率空间对应大栅格运动前的位置、全概率空间对应大栅格运动后位置和运动控制向量，即将大栅格运动前对应位姿，运动后对应位姿/>，输入到里程计运动模型得到式（2）：
；
表示大栅格运动时首先旋转的角度，/>大栅格旋转后沿此方向平移的距离，/>表示大栅格再次旋转的角度，/>表示运动前大栅格在X轴上的坐标，/>表示运动前大栅格在Y轴上的坐标，/>表示运动前小栅格的角度，/>表示运动后大栅格在X轴上的坐标，/>表示运动后大栅格在Y轴上的坐标，/>表示运动后大栅格的角度。
大栅格运动前位置达到大栅格运动后位置的概率P，即从一个位姿移动到另一个位姿的联合误差概率，也是从一个贝叶斯栅格落在另一个贝叶斯栅格中所发生的概率，通过式（3）进行闭式计算得到概率P，
；
其中，表示平移运动中平移分量的噪声，/>表示平移运动中旋转分量的噪声，表示旋转运动中平移分量的噪声，/>表示旋转运动中旋转分量的噪声，参数/>表示里程计的误差参数，用于确定里程计的精确度，越不精确，参数越大。
通过对每个小栅格进行计算，得到每个小栅格对应的概率，即第一个小栅格对应概率/>，第二个对应概率/>，第/>个小栅格对应概率/>，/>为小栅格总数量。
获得各小栅格的概率后，再将全概率空间运动后的每个小栅格中的概率进行统计后，按照栅格所含概率大小重新分配小栅格对应概率，重新分配概率后第个小栅格对应概率/>可以表示为：
；
步骤S5、在重新分配小栅格对应概率后的贝叶斯全概率空间中引入当前的点云匹配定位信息作为观测值，计算每个小栅格的权重值后与该栅格对应的概率结合，形成该栅格最终的选择概率，比较整个全概率空间中各小栅格的选择概率，选出概率最高者作为本次最优位姿输出。
在本步骤中，第个小栅格的权重/>计算方式如下：
；
上式中，为全概率空间运动后对应栅格的位姿信息/>轴，/>为全概率空间运动后对应栅格的位姿信息/>轴，/>为全概率空间运动后对应栅格后的位姿信息角度，/>、、/>为点云对应的位姿信息，/>、/>、/>为/>，/>、/>上对应的重要性系数，通过式（4）计算出各栅格所对应的权重值，与该栅格对应的概率结合，形成该小栅格最终的选择概率，以第/>个栅格为例，其最终选择概率/>计算方式如下：
。
步骤S6、重复执行步骤S3-S5，进行车辆位置及对应全概率空间的迭代求解以不断完善和优化车辆的定位精度，能够为自动驾驶车提供连续、平滑、实时的高精度定位信息。

Claims (7)

1.一种基于多分辨率贝叶斯网格的融合定位方法，其特征在于，包括如下步骤：
S1、获取车辆里程计信息以及激光雷达数据；
S2、对激光雷达数据进行拼接以及特征提取，通过点云匹配得到车辆的初始位姿，在初始位姿附近构建一个贝叶斯全概率空间；
S3、从车辆里程计信息获取运动控制向量，与车辆初始位姿一起输入里程计运动模型进行运动估计，得到一个预测的车辆位置，同时，全概率空间随车辆位置变动转移到预测位置附近；
S4、依次计算上一次全概率空间中每个大栅格运动到当前全概率空间中单个大栅格的概率，再用同样的方式计算大栅格中的小栅格概率，将全概率空间运动后的每个小栅格中的概率进行统计后，按照小栅格所含概率大小重新分配小栅格对应概率；
S5、在重新分配概率后的贝叶斯全概率空间中引入当前的点云匹配定位信息作为观测值，计算每个小栅格的权重值后与该小栅格对应的概率结合，形成该小栅格最终的选择概率，比较整个全概率空间中各小栅格的选择概率，选出概率最高者作为本次最优位姿输出；
S6、重复执行步骤S3-S5，进行车辆位置及对应全概率空间的迭代求解以实现车辆的高精度定位。
2.根据权利要求1所述的一种基于多分辨率贝叶斯网格的融合定位方法，其特征在于，步骤S1中，利用车辆中的CAN口与车载里程计进行数据交换以获取里程计信息，利用车辆的网口接收激光雷达数据，再通过交换机传输到车辆计算单元。
3.根据权利要求1所述的一种基于多分辨率贝叶斯网格的融合定位方法，其特征在于，步骤S2中，在初始位姿附近构建一个贝叶斯全概率空间，并根据精度需求调整该空间的大、小栅格分辨率，每个栅格中的概率均等，车辆在各方向上的位姿信息对应于栅格在全概率空间中的X、Y、Z轴上的坐标。
4.根据权利要求1所述的一种基于多分辨率贝叶斯网格的融合定位方法，其特征在于，步骤S3中，从车辆里程计信息获取运动控制向量，与车辆初始位姿一起输入到里程计运动模型中进行运动估计；里程计运动模型表示车辆由初始位姿x t-1= [ X Y θ ]T运动至预测位姿x t = [ X ′ Y ′ θ ′ ]T可拆分为车辆首先旋转角度δrot1，再沿着此方向平移δ trans，随后再次旋转角度δrot2抵达目标位姿，表达式如下：
；
上式，X表示初始位姿栅格在X轴上的坐标，Y表示初始位姿栅格在Y轴上的坐标，θ表示初始位姿栅格的角度，X ′表示预测位姿栅格在X轴上的坐标，Y ′表示预测位姿栅格在Y轴上的坐标，表示预测位姿栅格的角度。
5.根据权利要求4所述的一种基于多分辨率贝叶斯网格的融合定位方法，其特征在于，步骤S4中，依次计算上一次全概率空间中每个大栅格运动到当前全概率空间中单个大栅格的概率，具体包括：
在里程计运动模型中加入全概率空间对应大栅格运动前的位姿、运动后的位姿和运动控制向量，大栅格运动前对应位姿，运动后对应位姿/>，输入到里程计运动模型得到：
；
表示大栅格运动时首先旋转的角度，/>大栅格旋转后沿此方向平移的距离，表示大栅格再次旋转的角度，/>表示运动前大栅格在X轴上的坐标，/>表示运动前大栅格在Y轴上的坐标，/>表示运动前大栅格的角度，/>表示运动后大栅格在X轴上的坐标，/>表示运动后大栅格在Y轴上的坐标，/>表示运动后大栅格的角度；
然后进行闭式计算得到大栅格运动前位置到达大栅格运动后位置的概率P，P计算方式如下：
；
其中，表示平移运动中平移分量的噪声，/>表示平移运动中旋转分量的噪声，/>表示旋转运动中平移分量的噪声，/>表示旋转运动中旋转分量的噪声。
6.根据权利要求1所述的一种基于多分辨率贝叶斯网格的融合定位方法，其特征在于，步骤S5中，每个小栅格的权重值计算方式如下：
；
上式中，为全概率空间运动后对应小栅格的位姿信息/>轴，/>为全概率空间运动后对应小栅格的位姿信息/>轴，/>为全概率空间运动后对应小栅格后的位姿信息角度，/>、/>、/>为点云对应的位姿信息，/>，/>，/>为/>，/>，/>上对应的重要性系数，/>为第/>个小栅格的权重。
7.根据权利要求1所述的一种基于多分辨率贝叶斯网格的融合定位方法，其特征在于，步骤S5中，选择概率计算方式如下：
重新分配概率后第个小栅格对应概率/>表示为：
；
其中，表示小栅格数量参数，
将重新分配概率后第个小栅格对应概率/>与对应的权重/>结合形成第/>个小栅格对应的最终选择概率/>，
。
CN202311585869.2A 2023-11-27 2023-11-27 一种基于多分辨率贝叶斯网格的融合定位方法 Active CN117289294B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202311585869.2A CN117289294B (zh) 2023-11-27 2023-11-27 一种基于多分辨率贝叶斯网格的融合定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202311585869.2A CN117289294B (zh) 2023-11-27 2023-11-27 一种基于多分辨率贝叶斯网格的融合定位方法

Publications (2)

Publication Number Publication Date
CN117289294A CN117289294A (zh) 2023-12-26
CN117289294B true CN117289294B (zh) 2024-03-15

Family

ID=89252117

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202311585869.2A Active CN117289294B (zh) 2023-11-27 2023-11-27 一种基于多分辨率贝叶斯网格的融合定位方法

Country Status (1)

Country Link
CN (1) CN117289294B (zh)

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110689576A (zh) * 2019-09-29 2020-01-14 桂林电子科技大学 一种基于Autoware的动态3D点云正态分布AGV定位方法
CN110927740A (zh) * 2019-12-06 2020-03-27 合肥科大智能机器人技术有限公司 一种移动机器人定位方法
CN111486855A (zh) * 2020-04-28 2020-08-04 武汉科技大学 一种具有物体导航点的室内二维语义栅格地图构建方法
CN112313477A (zh) * 2018-06-22 2021-02-02 马瑞利欧洲公司 一种车辆环境映射的方法以及相应的系统、车辆和计算机程序产品
CN112923933A (zh) * 2019-12-06 2021-06-08 北理慧动（常熟）车辆科技有限公司 一种激光雷达slam算法与惯导融合定位的方法

Family Cites Families (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108732582B (zh) * 2017-04-20 2020-07-10 百度在线网络技术（北京）有限公司 车辆定位方法和装置
EP3734388B1 (en) * 2019-04-29 2023-03-15 Commissariat à l'Energie Atomique et aux Energies Alternatives Method and apparatus for performing simultaneous localization and mapping

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112313477A (zh) * 2018-06-22 2021-02-02 马瑞利欧洲公司 一种车辆环境映射的方法以及相应的系统、车辆和计算机程序产品
CN110689576A (zh) * 2019-09-29 2020-01-14 桂林电子科技大学 一种基于Autoware的动态3D点云正态分布AGV定位方法
CN110927740A (zh) * 2019-12-06 2020-03-27 合肥科大智能机器人技术有限公司 一种移动机器人定位方法
CN112923933A (zh) * 2019-12-06 2021-06-08 北理慧动（常熟）车辆科技有限公司 一种激光雷达slam算法与惯导融合定位的方法
CN111486855A (zh) * 2020-04-28 2020-08-04 武汉科技大学 一种具有物体导航点的室内二维语义栅格地图构建方法

Non-Patent Citations (5)

* Cited by examiner, † Cited by third party
Title
CUDA Implementation of A Parallel Particle Filter for Mobile Robot Pose Estimation;Bo Yan;2019 14th IEEE Conference on Industrial Electronics and Applications (ICIEA);全文 *
Simultaneous Localization and Mapping of Mobile Robot using GMapping Algorithm;C Marshal Revanth;2020 IEEE International Symposium on Smart Electronic Systems (iSES) (Formerly iNiS);全文 *
基于多假设粒子群优化的蒙特卡罗定位算法;安婷婷;肖南峰;;计算机工程与设计(08);全文 *
服务机器人室内环境层次化地图构建方法研究;霍光磊;中国博士学位论文全文数据库 信息科技辑;全文 *
移动机器人离散空间粒子滤波定位;李天成;孙树栋;高扬;;机械工程学报(19);全文 *

Also Published As

Publication number Publication date
CN117289294A (zh) 2023-12-26

Similar Documents

Publication Publication Date Title
CN112268559B (zh) 复杂环境下融合slam技术的移动测量方法
CN112525202A (zh) 一种基于多传感器融合的slam定位导航方法及系统
CN110243358A (zh) 多源融合的无人车室内外定位方法及系统
CN112347840A (zh) 视觉传感器激光雷达融合无人机定位与建图装置和方法
CN109341706A (zh) 一种面向无人驾驶汽车的多特征融合地图的制作方法
CN107690840B (zh) 无人机视觉辅助导航方法及系统
CN113406682B (zh) 一种定位方法、装置、电子设备及存储介质
CN111288989B (zh) 一种小型无人机视觉定位方法
CN110187375A (zh) 一种基于slam定位结果提高定位精度的方法及装置
CN114964212B (zh) 面向未知空间探索的多机协同融合定位与建图方法
CN112799096B (zh) 基于低成本车载二维激光雷达的地图构建方法
CN113108773A (zh) 一种融合激光与视觉传感器的栅格地图构建方法
CN102506867B (zh) 基于Harris角点匹配的SINS/SMANS组合导航方法及系统
CN114459470A (zh) 基于多传感器融合的巡检机器人定位方法
CN112833892B (zh) 一种基于轨迹对齐的语义建图方法
CN103901891A (zh) 一种基于层次结构的动态粒子树slam算法
CN114047766B (zh) 面向室内外场景长期应用的移动机器人数据采集系统及方法
CN115728803A (zh) 一种城市驾驶车辆连续定位系统及方法
Li et al. Loosely coupled GNSS/INS integration based on factor graph and aided by ARIMA model
CN116908777A (zh) 基于显式通信带标签伯努利的多机器人随机组网协同导航方法
CN116429116A (zh) 一种机器人定位方法及设备
CN116929338A (zh) 地图构建方法、设备及存储介质
Liu et al. Pseudolite constellation optimization for seamless train positioning in GNSS-challenged railway stations
CN114608568A (zh) 一种基于多传感器信息即时融合定位方法
CN117685953A (zh) 面向多无人机协同定位的uwb与视觉融合定位方法及系统

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant