CN112129297A - 一种多传感器信息融合的自适应校正室内定位方法 - Google Patents
一种多传感器信息融合的自适应校正室内定位方法 Download PDFInfo
- Publication number
- CN112129297A CN112129297A CN202011027020.XA CN202011027020A CN112129297A CN 112129297 A CN112129297 A CN 112129297A CN 202011027020 A CN202011027020 A CN 202011027020A CN 112129297 A CN112129297 A CN 112129297A
- Authority
- CN
- China
- Prior art keywords
- vehicle
- positioning
- odometer
- model
- adaptive
- 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.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 38
- 238000012937 correction Methods 0.000 title claims abstract description 24
- 230000004927 fusion Effects 0.000 title claims abstract description 22
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 30
- 230000000694 effects Effects 0.000 claims abstract description 9
- 230000002093 peripheral effect Effects 0.000 claims abstract description 4
- 238000004364 calculation method Methods 0.000 claims description 12
- 239000011159 matrix material Substances 0.000 claims description 12
- 230000003044 adaptive effect Effects 0.000 claims description 11
- 230000007613 environmental effect Effects 0.000 claims description 9
- 239000000284 extract Substances 0.000 claims description 3
- 238000005259 measurement Methods 0.000 description 8
- 238000012360 testing method Methods 0.000 description 8
- 238000002474 experimental method Methods 0.000 description 6
- 230000008569 process Effects 0.000 description 6
- 238000005516 engineering process Methods 0.000 description 5
- 238000011160 research Methods 0.000 description 5
- 238000010586 diagram Methods 0.000 description 3
- 238000004458 analytical method Methods 0.000 description 2
- 238000010276 construction Methods 0.000 description 2
- 238000001514 detection method Methods 0.000 description 2
- 238000000605 extraction Methods 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 230000006855 networking Effects 0.000 description 2
- 238000005457 optimization Methods 0.000 description 2
- 239000002245 particle Substances 0.000 description 2
- 238000005070 sampling Methods 0.000 description 2
- 238000012216 screening Methods 0.000 description 2
- 239000000126 substance Substances 0.000 description 2
- 230000002238 attenuated effect Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000000903 blocking effect Effects 0.000 description 1
- 238000007621 cluster analysis Methods 0.000 description 1
- 238000007796 conventional method Methods 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000002068 genetic effect Effects 0.000 description 1
- 238000007689 inspection Methods 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 238000007500 overflow downdraw method Methods 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 230000001737 promoting effect Effects 0.000 description 1
- 230000009467 reduction Effects 0.000 description 1
- 238000004088 simulation Methods 0.000 description 1
- 238000000638 solvent extraction Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 238000012546 transfer Methods 0.000 description 1
- 230000009466 transformation Effects 0.000 description 1
Images
Classifications
-
- 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
- G01C21/206—Instruments for performing navigational calculations specially adapted for indoor navigation
-
- 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/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
-
- 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/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
-
- 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/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/18—Stabilised platforms, e.g. by gyroscope
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
Abstract
本发明涉及一种多传感器信息融合的自适应校正室内定位方法，属于车联网与自动驾驶领域。该方法包括：S1：根据车辆自身角速度信息，采用自适应里程计滑移误差补偿算法，实现对里程计滑移效应的补偿及补偿参数自整定；S2：构建惯导与里程计融合定位的算法模型；S3：构建环境特征模型，即利用传感器提取周边固有环境特征，结合惯导与里程计融合定位的算法模型解算得到姿态角，从而推算得到车辆的全局坐标位置。本发明提高了系统的定位精度，并利用简单的结构实现车辆的全局定位。
Description
技术领域
本发明属于车联网与自动驾驶领域，涉及一种多传感器信息融合的自适应校正室内定位方法。
背景技术
目前，随着车联网与自动驾驶等领域的相关技术取得突破性发展，实现对运动车辆准确位置信息的及时获取与高效处理已成为当今智能交通领域的一大研究热点，尤其在自动驾驶等领域，车辆的定位精度需求一般在亚米级别，很多时候甚至需要达到厘米级别。高精度定位技术是实现车联网、自动驾驶及自动泊车等的关键性技术之一。
目前全球卫星定位技术在车辆室外定位的应用上已经十分醇熟，在宽阔的室外场景下，卫星定位精度可达米级，甚至在特定场景下最高可达到厘米级。但是卫星定位系统对待定位设备所处空间有着一定的要求，地面设备往往需要接收到来自至少四颗卫星的信号才能完成精确的定位，一旦搜星过程中发生物理阻隔或遮挡等，卫星定位系统所依赖的无线电信号就会发生衰减和散射，精度也会大打折扣。然而如今地面建筑的覆盖范围越来越广泛，在很多情况下车辆均处于室内环境，例如环境复杂的室内停车场、地下车库及隧道等，在此种环境下，无线电信号无法完全穿透墙体等建筑设施。考虑到非自主式定位方式的局限性，基于惯导与多传感器信息融合的导航技术开始得到广泛的研究和应用。
基于惯导与多传感器信息融合的定位方式选取不依赖无线电信号，且自身对环境光不敏感的自主屏蔽式惯导模块作为主要的定位装置，并利用里程计对惯导进行融合校正。但随着时间的推移，里程计会因为车辆的滑移问题出现累计误差，进而影响系统的定位结果。
现有技术中针对车辆的滑移补偿方案比较少，贝旭颖等人利用传统UMBmark或优化后的该方法进行误差补偿的方案，其综合考虑三种主要系统误差来源产生的误差对移动机器人直线运动和定点旋转运动造成的影响，同时采用正方形回路终点的方向误差代替位置误差来校核系统参数。该方法需要误差已经对车辆造成影响后，再进行参数调整与补偿，有明显的迟滞性，不适用于实际应用中移动车辆的使用。公开号为CN109269500A的专利申请(名称：一种基于惯性导航系统和里程计的管道定位方法和系统)，公开了采用十二位置标定和x,y,z三轴速率标定方法，标定时要使安装了系统平台的转台到达指定位置或指定速率，时间约30分钟，尽管该方案中也利用了惯导系统，但是与发明提出的应用角速度的方法有明显区别，且该方法测试时间较长，不适用于实时的里程计补偿与使用。上述方案均没有充分利用组合导航系统信息，在实际工作中难以进行实时自更新与检验。
完成了里程计的补偿，为了解决惯性导航最大的问题——随着时间的推移，IMU模块的误差会产生累积，必须准确而及时地对其进行校准。但是在室内停车场环境中，常用的基于无线电信号RSSI的校正方式，像蓝牙、WiFi、ZigBee等，这类方式的无线电信号在环境复杂的室内停车场不仅容易发散，而且需要进行大量的信标部署，费时费力，而基于视觉、可见光等定位方式由于算法的复杂性和停车场极不稳定的光信号，也不具备适用性。而一般的进行地图构建的方法(基于激光地图、超声波模块)，提取出的特征数量比较庞大，需要从众多点、线特征中筛选出目标泊车位特征，使用现有方法筛选流程较为复杂。
目前环境特征提取主要使用激光雷达、超声波测距模块等，激光雷达应用广泛，东南大学的章小兵、宋爱国等人在《基于视觉和超声波传感器的地面移动机器人定位与路径规划研究》提出了利用超声波传感器对室内空间分区的方法，在室内有随机障碍物的情况下，根据空间区域特点剔除坏值,采用串联和并联推断的方式，对机器人进行定位，该方法是基于状态的融合方法，不注重某一个超声波传感器的测量精度提高，一定程度上可以消除里程计局部定位造成的误差，但是该方法一方面缺乏对于自身姿态信息的融合计算，只能在定速的确定状态下执行定位操作。另一种通过构建地图的定位方法，如哈尔滨工业大学的靳新辉在《移动机器人室内地图构建及定位方法的研究》提出的基于栅格地图，在初始位姿不确定的情况下，引入粒子滤波器对机器人位姿进行估计，作者将遗传算法思想引入粒子滤波器得到优化算法，提高了系统的定位精度，相比于超声波定位，利用红外定位传感器虽然可以得到更精确的绝对位置估计，但是因为缺乏对里程计的补偿环节，因此难以保证地图在定位过程中的重复精度，且该方法是一种理论上处理不确定信息的方法，在实际应用中并没有进行实际测试。
根据上述文献可知，目前的车辆位姿确定方案，有以EKF、UKF为基础进行优化的算法、以机器视觉为基础的特征识别方案和以IMU与全球定位系统融合的解算方案，但上述方案都存着在成本过高、系统本身复杂或者创新不足等问题，且没有对车辆本身的信息进行有效利用，不适用于室内/地下停车场车辆辅助定位的应用场景。
因此，亟需一种新的能够提高室内车辆定位的定位方法。
发明内容
有鉴于此，本发明的目的在于提供一种多传感器信息融合的自适应校正室内定位方法，在基于航迹推算的算法模型和IMU姿态解算模型基础上，利用EKF算法将里程计推算得到的位姿数据与IMU融合，提高了系统的定位精度；同时还采用自适应滑移补偿算法对里程计的误差进行补偿，进一步提高系统的定位精度。另外，本发明还利用简单的双超声波传感器对称分布探测模型，实现对环境特征的提取，并结合惯导融合以后解算得到姿态角推算得到车辆的全局坐标位置，利用简单的结构实现车辆的全局定位。
为达到上述目的，本发明提供如下技术方案：
一种多传感器信息融合的自适应校正室内定位方法，具体包括以下步骤：
S1：根据车辆自身角速度信息，采用自适应里程计滑移误差补偿算法，实现对里程计滑移效应的补偿及补偿参数自整定；
S2：构建惯导与里程计融合定位的算法模型；
S3：构建环境特征模型，即利用传感器提取周边固有环境特征，结合惯导与里程计融合定位的算法模型解算得到姿态角，从而推算得到车辆的全局坐标位置。
进一步，步骤S1中，所述自适应里程计滑移误差补偿算法，具体包括以下步骤：
S11：构建里程计模型，并根据该模型计算得到移动车辆的位姿信息；
S12：根据当前时刻车辆的左右轮速、行驶时间与轮距补偿系数，得到下一时刻车辆的偏航角度；
S13：将惯导模块的输出角速度作为参考角速度，将其解算出来的陀螺仪角速度作为参考值实现校正，对车辆的左右轮速通过编码器实现闭环PID控制，由此能够降低里程计补偿模型参数整定的难度，提高自适应性。
进一步，步骤S11中，构建的里程计模型为：
其中，车辆在k时刻的位置与姿态表示为矩阵Xk＝[xk yk θk]T，k+1时刻的车辆位置与姿态表示为矩阵Xk+1＝[xk+1 yk+1 θk+1]T，其对应的增量分别为Δxk，Δyk与Δθk；ΔdL、ΔdR和ΔSk分别表示间隔时间Δt内车辆左右驱动轮行驶的距离以及汽车的运行距离，Δθk表示车辆转动的偏航角度，R表示车辆转过的半径，L表示两轮之间的距离，r表示驱动轮半径，P表示编码器单圈的脉冲数，N表示时间Δt内获取得到的脉冲总数。
进一步，步骤S12中，根据当前时刻汽车的左右轮速、行驶时间与轮距补偿系数，得到下一时刻汽车的偏航角度为：
将汽车的位姿转换到全局坐标系中有：
其中，Di为汽车定位传感器位置坐标到小车中心坐标的直线距离。
进一步，步骤S3中，构建的环境特征模型一为：极限情况下，假定车辆以直线状态向前行驶时，位姿较为稳定，未发生偏移，此时如果车辆同时提取完毕左右两边的柱子信息，匹配发现符合预设的线段特征信息；此时车辆的全局位姿模型表示为：
其中，(x,y)表示车辆参考点的全局坐标，(x′e,y′e)与(xe,ye)分别为左右两侧柱子提取出来的特征线段的终点坐标，b为车辆定位参考点到超声波传感器的水平距离，l′与l分别为左右两侧超声波测得的距(x′e,y′e)与(xe,ye)两点各自的距离值，当两边同时匹配完成时n＝2，否则n＝1。
进一步，步骤S3中，构建的环境特征模型二为：正常转向状态下驶出柱间，在正常转向时，车辆的航向角会发生偏移，偏移角度即航向角θ，在此情况下，车辆无法同时探测到两个对称的特征点，只能利用其中一个点进行位置推算。此时，车辆在全局坐标系中的位置计算公式为：
进一步，步骤S3中，构建的环境特征模型三为：正常转向状态下驶入柱间，车辆在行驶时，左侧已经提前完成线段特征的辨识与匹配，此时根据左侧提取到的特征信息进行位置计算，其坐标表达式为：
本发明的有益效果在于：
(1)本发明针对车辆在实际运动中会产生滑移误差的情况，提出了根据车辆自身高精度角速度信息作为补偿的方法，设计了定位系统的自适应里程计滑移误差补偿算法，提高了滑移误差的计算精度。
(2)本发明充分研究了室内停车场固有特征(室内停车场存在诸多固有环境特征，比如柱子、其尺寸往往相等，在室内停车场中一般呈两侧对称分布，且柱子与柱子之间的距离较为均匀)，能够作为明显的特征标志物提取，进而提出了利用简单的双超声波传感器对称分布探测模型，实现对环境特征的提取，并结合惯导融合以后解算得到姿态角推算得到车辆的全局坐标位置，即本发明仅仅利用简单的结构实现了车辆的全局定位，大大提高了室内定位的精准性。
(3)本发明采用的多传感器信息融合方法相对于单纯的惯导和里程计定位，超声波的匹配定位方式定位精度已经得到极大改善，通过对超声波模块进行前提标定补偿，进一步消除了测量误差。
本发明的其他优点、目标和特征在某种程度上将在随后的说明书中进行阐述，并且在某种程度上，基于对下文的考察研究对本领域技术人员而言将是显而易见的，或者可以从本发明的实践中得到教导。本发明的目标和其他优点可以通过下面的说明书来实现和获得。
附图说明
为了使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明作优选的详细描述，其中：
图1为自适应里程计滑移补偿算法的控制框图；
图2为实验小车里程计运动模型示意图；
图3为里程计航向角校正实验效果图，其中(a)为测试里程计航向角校正结果的场景图；(b)为校正前后的里程计航向角测试结果对比图；
图4为极限状态下的车辆位置示意图；
图5为一般状态下(车辆驶出)的车辆位置示意图；
图6为一般状态下(车辆驶入)的车辆位置示意图；
图7为超声波模块定位测试场景示意图；
图8为超声波模块定位结果示意图。
具体实施方式
以下通过特定的具体实例说明本发明的实施方式，本领域技术人员可由本说明书所揭露的内容轻易地了解本发明的其他优点与功效。本发明还可以通过另外不同的具体实施方式加以实施或应用，本说明书中的各项细节也可以基于不同观点与应用，在没有背离本发明的精神下进行各种修饰或改变。需要说明的是，以下实施例中所提供的图示仅以示意方式说明本发明的基本构想，在不冲突的情况下，以下实施例及实施例中的特征可以相互组合。
请参阅图1～图8，本发明提供了一种多传感器信息融合的自适应校正室内定位方法，包括以下步骤：
S1：根据车辆自身角速度信息，采用自适应里程计滑移误差补偿算法，实现对里程计滑移效应的补偿及补偿参数自整定；如图1所示，具体包括：
S11：构建里程计模型，并根据该模型计算得到移动车辆的位姿信息。
本实验使用的是Minibalance研制的带霍尔编码器的电机，电机减速比为30，其编码器精度为390，且成本低廉，比较适合于大多数的场景应用，足够用于对两驱动轮的小车进行位姿推断与估计，其运动学模型如图2所示。
根据编码器的工作原理，在编码器单圈脉冲数与一定时间间隔Δt内的脉冲总数已知的情况下，则可以获取到驱动轮的转速，从而推算出轮子的移动距离。因此，预先设定ROS实验小车左、右两轮间的宽度距离为L，驱动轮半径为r，霍尔编码器单圈的脉冲数为P，Δt时间获取得到的脉冲总数为N，左右某一个驱动轮在Δt时间内行驶的距离表示为Δd，则Δd的计算形式如式(1)所示。
从图2中可知，ROS实验小车在k时刻的位置与姿态可表示为矩阵Xk＝[xk yk θk]T，k+1时刻的机器人位置姿态表示为矩阵Xk+1＝[xk+1 yk+1 θk+1]T，其对应的增量分别为Δxk，Δyk与Δθk，其中，ROS小车左驱动轮的运动距离设为ΔdL，右轮为ΔdR，结合左右两轮的编码器数据信息，可得Δt间隔时间内小车的运行距离为ΔSk，小车转动的偏航角度为Δθk，其转过的半径为R，并且前面已设定两轮之间的距离为L，则有：
综合以上各式可知：
进一步推算出在k+1时刻有：
将小车的位姿转换到全局坐标系中有：
其中，Di为小车定位传感器位置坐标到小车中心坐标的直线距离。本实验中对应小车里程计的各项固有参数如表1所示。
表1 ROS实验小车平台里程计核心参数表
S12：根据实际条件下当前时刻车辆的左右轮速、行驶时间与轮距补偿系数，得到下一时刻车辆的偏航角度。
S13：将惯导模块的输出角速度作为参考角速度，将其解算出来的陀螺仪角速度作为参考值实现校正，对车辆的左右轮速通过编码器实现闭环PID控制，由此能够降低里程计补偿模型参数整定的难度，提高自适应性。
验证步骤S1的补偿效果：采用自适应滑移补偿算法对惯导与里程计的融合定位算法进行优化，在实际场景中进行了测试，测试场景如图3(a)所示，车辆初始航向角为零，控制车辆以0.25m/s的速度沿半径约1.5m的圆进行逆时针行驶，得到校正前后的里程计航向角如图3(b)所示。
实验选取航向角理论度数应为90°、180°和-90°处三个采样点进行数据采集分析，通过测试得到的里程计航向角度数如表2所示。从得到的数据分析可知，校正后车辆系统的里程计航向角误差得到改善。
表2校正前后里程计航向角值对比表
| 校正过程 | 90° | 180° | -90° |
| 校正前 | 76.01° | 171.25° | -94.82° |
| 校正后 | 84.43° | 179.74° | -87.40° |
S2：基于扩展卡尔曼滤器(Extended Kalman Filter,EKF)构建惯导与里程计融合定位的算法模型。
针对非线性的离散系统，EKF算法研究的模型如式(8)所示。
其中，
是系统的状态变量，f代表状态转移函数，
是系统的观测值，h代表测量函数，wk以及vk均为零均值高斯白噪声，并且满足式(9)～(10)的条件。
在上述各式中，Qk与Rk分别为系统噪声与测量噪声的方差阵。
当系统中wk-1和vk均为零时，将状态方程(8)按照一阶泰勒级数展开，其形式如式(11)所示。
上式中，Fk、Hk分别为雅克比矩阵，形式如式(12)～(13)所示。
EKF算法的具体的实现步骤为：
Step1:首先完成初始化操作：
其中，
为初始状态估计，P0为初始估计误差协方差阵。
Step2:计算式(12)所示的系统状态偏微分矩阵。
Step3:完成时间更新过程，也就是计算状态估计和对应的误差协方差阵，如式(15)～(16)所示。
Pk|k-1＝FkPk-1Fk T+Qk (16)
Step4:计算式(13)所示的测量方程偏微分矩阵。
Step5:完成系统状态更新，也就是计算系统的卡尔曼增益系数，更新系统的观测量和对应的误差协方差矩阵，如式(17)～(19)所示。
Pk＝Pk|k-1-KkHkPk|k-1 (19)
当系统为非线性系统，并且可以通过线性化得到较好的近似效果时，那么扩展卡尔曼滤波器是用于状态估算的一个很好的选择。并且在运算性能上，一阶EKF算法的计算速度较快，实时性好，且精度高。
S3：构建环境特征模型，即利用传感器提取周边固有环境特征，结合惯导与里程计融合定位的算法模型解算得到姿态角，从而推算得到车辆的全局坐标位置。
对实际停车场的特征分布进行分析，大部分的地下停车场，都有大量分布均匀、尺寸相同的承重柱，无较大偏差，因此将所有承重柱预先在全局地图中配置为正确的坐标，同时引入Split-Merge算法将超声波模块提取得到数据点特征不断进行线段拟合和聚类分析，将拟合得到的线段与预设的柱子特征线段进行匹配，从而判断所探测到的目标物是否为柱子，若是柱子则开始定位过程，并在实际的测试过程中，得到了三种具体定位场景，相对于单纯的惯导和里程计定位，超声波的匹配定位方式定位精度已经得到极大改善，通过对超声波模块进行前提标定补偿，可以进一步消除测量误差。
构建环境特征模型，具体包括以下三种模型：
模型一：该模型为极限情况，如图4所示：假定车辆以直线状态向前行驶时，位姿较为稳定，未发生偏移，此时如果车辆同时提取完毕左右两边的柱子信息，匹配发现符合预设的线段特征信息。此时车辆的全局位姿模型如式(20)所示。
其中，(x,y)为车辆参考点的全局坐标表示，(x′e,y′e)与(xe,ye)分别为左右两侧柱子提取出来的特征线段的终点坐标，b为车辆定位参考点到超声波传感器的水平距离，l′与l分别为左右两侧超声波测得的距(x′e,y′e)与(xe,ye)两点各自的距离值，当两边同时匹配完成时n＝2，否则n＝1。
模型二：正常转向状态(驶出柱间)，如图5所示：在正常转向时，车辆的航向角会发生偏移，偏移角度即航向角θ，在此情况下，车辆无法同时探测到两个对称的特征点，只能利用其中一个点进行位置推算。此时，车辆在全局坐标系中的位置计算如式(21)所示。
模型三：正常转向状态(驶入柱间)，如图6所示：为了增强系统的定位性能，此时考虑如图6的情景，车辆在行驶时，左侧已经提前完成线段特征的辨识与匹配，此时可以根据左侧提取到的特征信息进行位置计算。其坐标表达式如式(22)所示。
验证构建的环境特征模型的筛选效果：
根据上述构建的模型，搭建如图7所示的模拟场景对基于惯导更新以后偏航角的位置识别算法进行验证，为模拟真实室内停车场环境，实验场景两边放有分布对称的障碍物，此处使用四个大小一致的纸箱模拟障碍物，纸箱的尺寸为40cm×30cm×45cm，其中，p1～p8分别为定位系统特征识别过程中四个障碍物的匹配起止点，各个点的坐标在设定的全局坐标系中为已知。
让车辆按照直线方向进行运动以验证定位性能，开始定位时，设定运动轨迹上的两个点(1.45，0.9)与(1.45，2.3)为采样点，进行30次左右的定位实验，在这两个点处对车辆的定位效果进行分析，得到的数据结果如图8所示。
尽管设定车辆的运动方向为垂直运动，但是在实际的运动过程中，车辆的角度仍然不可避免的会发生少量的偏差，得到的定位相关数据如表2所示。
表2超声波模块定位误差
实验结果表明，提取特征拟合得到的线段与预设线段长度基本一致，本发明所提方法相对于单纯的惯导和里程计定位，超声波的匹配定位方式定位精度已经得到极大改善，通过对超声波模块进行前提标定补偿，可以进一步消除测量误差。
最后说明的是，以上实施例仅用以说明本发明的技术方案而非限制，尽管参照较佳实施例对本发明进行了详细说明，本领域的普通技术人员应当理解，可以对本发明的技术方案进行修改或者等同替换，而不脱离本技术方案的宗旨和范围，其均应涵盖在本发明的权利要求范围当中。
Claims (7)
1.一种多传感器信息融合的自适应校正室内定位方法，其特征在于，该方法具体包括以下步骤：
S1：根据车辆自身角速度信息，采用自适应里程计滑移误差补偿算法，实现对里程计滑移效应的补偿及补偿参数自整定；
S2：构建惯导与里程计融合定位的算法模型；
S3：构建环境特征模型，即利用传感器提取周边固有环境特征，结合惯导与里程计融合定位的算法模型解算得到姿态角，从而推算得到车辆的全局坐标位置。
2.根据权利要求1所述的自适应校正室内定位方法，其特征在于，步骤S1中，所述自适应里程计滑移误差补偿算法，具体包括以下步骤：
S11：构建里程计模型，并根据该模型计算得到移动车辆的位姿信息；
S12：根据当前时刻车辆的左右轮速、行驶时间与轮距补偿系数，得到下一时刻车辆的偏航角度；
S13：将惯导模块的输出角速度作为参考角速度，将其解算出来的陀螺仪角速度作为参考值实现校正，对车辆的左右轮速通过编码器实现闭环PID控制。
3.根据权利要求2所述的自适应校正室内定位方法，其特征在于，步骤S11中，构建的里程计模型为：
其中，车辆在k时刻的位置与姿态表示为矩阵Xk＝[xk yk θk]T，k+1时刻的车辆位置与姿态表示为矩阵Xk+1＝[xk+1 yk+1 θk+1]T，其对应的增量分别为Δxk，Δyk与Δθk；ΔdL、ΔdR和ΔSk分别表示间隔时间Δt内车辆左右驱动轮行驶的距离以及汽车的运行距离，Δθk表示车辆转动的偏航角度，R表示车辆转过的半径，L表示两轮之间的距离，r表示驱动轮半径，P表示编码器单圈的脉冲数，N表示时间Δt内获取得到的脉冲总数。
4.根据权利要求3所述的自适应校正室内定位方法，其特征在于，步骤S12中，根据当前时刻汽车的左右轮速、行驶时间与轮距补偿系数，得到下一时刻汽车的偏航角度为：
将汽车的位姿转换到全局坐标系中有：
其中，Di为汽车定位传感器位置坐标到小车中心坐标的直线距离。
5.根据权利要求1所述的自适应校正室内定位方法，其特征在于，步骤S3中，构建的环境特征模型一为：极限情况下，假定车辆以直线状态向前行驶时，位姿稳定，未发生偏移，此时如果车辆同时提取完毕左右两边的柱子信息，匹配发现符合预设的线段特征信息；此时车辆的全局位姿模型表示为：
其中，(x,y)表示车辆参考点的全局坐标，(x′e,y′e)与(xe,ye)分别为左右两侧柱子提取出来的特征线段的终点坐标，b为车辆定位参考点到超声波传感器的水平距离，l′与l分别为左右两侧超声波测得的距(x′e,y′e)与(xe,ye)两点各自的距离值，当两边同时匹配完成时n＝2，否则n＝1。
6.根据权利要求5所述的自适应校正室内定位方法，其特征在于，步骤S3中，构建的环境特征模型二为：正常转向状态下驶出柱间，在正常转向时，车辆的航向角会发生偏移，偏移角度即航向角θ，在此情况下，车辆在全局坐标系中的位置计算公式为：
7.根据权利要求5所述的自适应校正室内定位方法，其特征在于，步骤S3中，构建的环境特征模型三为：正常转向状态下驶入柱间，车辆在行驶时，左侧已经提前完成线段特征的辨识与匹配，此时根据左侧提取到的特征信息进行位置计算，其坐标表达式为：
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011027020.XA CN112129297B (zh) | 2020-09-25 | 一种多传感器信息融合的自适应校正室内定位方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011027020.XA CN112129297B (zh) | 2020-09-25 | 一种多传感器信息融合的自适应校正室内定位方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN112129297A true CN112129297A (zh) | 2020-12-25 |
| CN112129297B CN112129297B (zh) | 2024-04-30 |
Family
ID=
Cited By (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112857393A (zh) * | 2021-01-30 | 2021-05-28 | 厦门攸信信息技术有限公司 | 一种麦克纳姆轮平台的平面定位及里程计量方法 |
| CN113203419A (zh) * | 2021-04-25 | 2021-08-03 | 重庆大学 | 基于神经网络的室内巡检机器人校正定位方法 |
| CN113568430A (zh) * | 2021-07-31 | 2021-10-29 | 北京晟天行科技有限公司 | 一种无人机机翼执行数据的修正控制方法 |
| CN113720343A (zh) * | 2021-08-16 | 2021-11-30 | 中国科学院上海微系统与信息技术研究所 | 基于动态数据实时适应的航向预测方法 |
| CN114018284A (zh) * | 2021-10-13 | 2022-02-08 | 上海师范大学 | 一种基于视觉的轮速里程计校正方法 |
| CN114166218A (zh) * | 2021-11-30 | 2022-03-11 | 上海智能网联汽车技术中心有限公司 | 基于多定位融合的室内定位导航系统及方法 |
| CN114296454A (zh) * | 2021-12-24 | 2022-04-08 | 大连理工大学人工智能大连研究院 | 一种全向全驱移动机器人的自适应运动控制方法及系统 |
| CN114563006A (zh) * | 2022-03-17 | 2022-05-31 | 长沙慧联智能科技有限公司 | 基于参考线匹配的车辆全局定位方法及装置 |
Citations (17)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2007333385A (ja) * | 2006-06-11 | 2007-12-27 | Toyota Central Res & Dev Lab Inc | 不動物位置記録装置 |
| DE102012016800A1 (de) * | 2012-08-23 | 2014-02-27 | Audi Ag | Verfahren und Vorrichtung zur Ermittlung einer Fahrzeugposition in einer kartierten Umgebung |
| CN107478214A (zh) * | 2017-07-24 | 2017-12-15 | 杨华军 | 一种基于多传感器融合的室内定位方法及系统 |
| CN108981692A (zh) * | 2018-06-14 | 2018-12-11 | 兰州晨阳启创信息科技有限公司 | 一种基于惯导/视觉里程计的列车定位方法及系统 |
| CN109596140A (zh) * | 2019-01-30 | 2019-04-09 | 东软睿驰汽车技术（沈阳）有限公司 | 一种车辆定位方法、自动驾驶控制方法及相关系统 |
| CN109739094A (zh) * | 2019-02-28 | 2019-05-10 | 哈尔滨理工大学 | 一种基于自适应滑模控制的移动机器人轨迹跟踪新方法 |
| CN110031019A (zh) * | 2019-04-18 | 2019-07-19 | 北京智行者科技有限公司 | 一种用于自动驾驶车辆的打滑检测处理方法 |
| JP2019124573A (ja) * | 2018-01-16 | 2019-07-25 | 株式会社豊田中央研究所 | 自車位置推定装置、自車位置推定方法、および自車位置推定プログラム |
| US20190259170A1 (en) * | 2018-02-21 | 2019-08-22 | Beijing Jingdong Shangke Information Technology Co., Ltd. | System and method for feature screening in slam |
| CN110221333A (zh) * | 2019-04-11 | 2019-09-10 | 同济大学 | 一种车载ins/od组合导航系统的量测误差补偿方法 |
| CN110243358A (zh) * | 2019-04-29 | 2019-09-17 | 武汉理工大学 | 多源融合的无人车室内外定位方法及系统 |
| CN110388925A (zh) * | 2018-04-17 | 2019-10-29 | 法拉第未来公司 | 用于与自动导航有关的车辆定位的系统和方法 |
| CN110514863A (zh) * | 2019-09-23 | 2019-11-29 | 北京智行者科技有限公司 | 一种用于无人车车轮打滑的判别及补偿方法 |
| US20190367012A1 (en) * | 2018-05-29 | 2019-12-05 | Hitachi Automotive Systems, Ltd. | Road marker detection method |
| CN111152834A (zh) * | 2020-01-10 | 2020-05-15 | 大连理工大学 | 一种基于阿克曼转向修正的电动汽车电子差速控制方法 |
| CN111272165A (zh) * | 2020-02-27 | 2020-06-12 | 清华大学 | 一种基于特征点标定的智能车定位方法 |
| CN111551186A (zh) * | 2019-11-29 | 2020-08-18 | 福瑞泰克智能系统有限公司 | 一种车辆实时定位方法、系统及车辆 |
Patent Citations (17)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2007333385A (ja) * | 2006-06-11 | 2007-12-27 | Toyota Central Res & Dev Lab Inc | 不動物位置記録装置 |
| DE102012016800A1 (de) * | 2012-08-23 | 2014-02-27 | Audi Ag | Verfahren und Vorrichtung zur Ermittlung einer Fahrzeugposition in einer kartierten Umgebung |
| CN107478214A (zh) * | 2017-07-24 | 2017-12-15 | 杨华军 | 一种基于多传感器融合的室内定位方法及系统 |
| JP2019124573A (ja) * | 2018-01-16 | 2019-07-25 | 株式会社豊田中央研究所 | 自車位置推定装置、自車位置推定方法、および自車位置推定プログラム |
| US20190259170A1 (en) * | 2018-02-21 | 2019-08-22 | Beijing Jingdong Shangke Information Technology Co., Ltd. | System and method for feature screening in slam |
| CN110388925A (zh) * | 2018-04-17 | 2019-10-29 | 法拉第未来公司 | 用于与自动导航有关的车辆定位的系统和方法 |
| US20190367012A1 (en) * | 2018-05-29 | 2019-12-05 | Hitachi Automotive Systems, Ltd. | Road marker detection method |
| CN108981692A (zh) * | 2018-06-14 | 2018-12-11 | 兰州晨阳启创信息科技有限公司 | 一种基于惯导/视觉里程计的列车定位方法及系统 |
| CN109596140A (zh) * | 2019-01-30 | 2019-04-09 | 东软睿驰汽车技术（沈阳）有限公司 | 一种车辆定位方法、自动驾驶控制方法及相关系统 |
| CN109739094A (zh) * | 2019-02-28 | 2019-05-10 | 哈尔滨理工大学 | 一种基于自适应滑模控制的移动机器人轨迹跟踪新方法 |
| CN110221333A (zh) * | 2019-04-11 | 2019-09-10 | 同济大学 | 一种车载ins/od组合导航系统的量测误差补偿方法 |
| CN110031019A (zh) * | 2019-04-18 | 2019-07-19 | 北京智行者科技有限公司 | 一种用于自动驾驶车辆的打滑检测处理方法 |
| CN110243358A (zh) * | 2019-04-29 | 2019-09-17 | 武汉理工大学 | 多源融合的无人车室内外定位方法及系统 |
| CN110514863A (zh) * | 2019-09-23 | 2019-11-29 | 北京智行者科技有限公司 | 一种用于无人车车轮打滑的判别及补偿方法 |
| CN111551186A (zh) * | 2019-11-29 | 2020-08-18 | 福瑞泰克智能系统有限公司 | 一种车辆实时定位方法、系统及车辆 |
| CN111152834A (zh) * | 2020-01-10 | 2020-05-15 | 大连理工大学 | 一种基于阿克曼转向修正的电动汽车电子差速控制方法 |
| CN111272165A (zh) * | 2020-02-27 | 2020-06-12 | 清华大学 | 一种基于特征点标定的智能车定位方法 |
Non-Patent Citations (9)
| Title |
|---|
| MIRABADI ET AL.: "Slip and slide detection and compensation for odometer system, using adaptive fuzzy kalman filter", SENSOR LETTERS, vol. 07, no. 01, pages 84 - 90 * |
| 宋小康 等: "全地形轮式移动机器人运动学建模与分析", 机械工程学报, no. 06, pages 148 - 154 * |
| 彭慕蓉 等: "采用粒子群算法的AGV路径跟踪控制研究", 机械设计与制造, no. 06, pages 20 - 23 * |
| 李琳辉 等: "基于滑转补偿的月球车轨迹跟踪控制算法", 电机与控制学报, vol. 18, no. 01, 31 January 2014 (2014-01-31), pages 105 - 111 * |
| 武萌: "基于运动学约束的履带通信车组合导航研究", 压电与声光, vol. 41, no. 01, pages 141 - 145 * |
| 王美玲 等: "一种车辆模型辅助的MEMS-SINS导航方法", 中国惯性技术学报, vol. 25, no. 02, 15 April 2017 (2017-04-15), pages 209 - 215 * |
| 闫浩月: "融合激光测距仪和惯导信息的移动机器人室内定位方法研究", 中国优秀硕士学位论文全文数据库信息科技辑, no. 2018, pages 140 - 936 * |
| 马智渊 等: "捷联惯导/里程计组合导航技术", 火力与指挥控制, vol. 42, no. 02, pages 183 - 186 * |
| 高文: "基于惯导与环境信息融合的室内停车场车辆定位技术研究", 中国优秀硕士学位论文全文数据库工程科技Ⅱ辑, no. 2022, pages 034 - 280 * |
Cited By (11)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112857393A (zh) * | 2021-01-30 | 2021-05-28 | 厦门攸信信息技术有限公司 | 一种麦克纳姆轮平台的平面定位及里程计量方法 |
| CN113203419A (zh) * | 2021-04-25 | 2021-08-03 | 重庆大学 | 基于神经网络的室内巡检机器人校正定位方法 |
| CN113203419B (zh) * | 2021-04-25 | 2023-11-10 | 重庆大学 | 基于神经网络的室内巡检机器人校正定位方法 |
| CN113568430A (zh) * | 2021-07-31 | 2021-10-29 | 北京晟天行科技有限公司 | 一种无人机机翼执行数据的修正控制方法 |
| CN113720343A (zh) * | 2021-08-16 | 2021-11-30 | 中国科学院上海微系统与信息技术研究所 | 基于动态数据实时适应的航向预测方法 |
| CN114018284A (zh) * | 2021-10-13 | 2022-02-08 | 上海师范大学 | 一种基于视觉的轮速里程计校正方法 |
| CN114018284B (zh) * | 2021-10-13 | 2024-01-23 | 上海师范大学 | 一种基于视觉的轮速里程计校正方法 |
| CN114166218A (zh) * | 2021-11-30 | 2022-03-11 | 上海智能网联汽车技术中心有限公司 | 基于多定位融合的室内定位导航系统及方法 |
| CN114296454A (zh) * | 2021-12-24 | 2022-04-08 | 大连理工大学人工智能大连研究院 | 一种全向全驱移动机器人的自适应运动控制方法及系统 |
| CN114563006A (zh) * | 2022-03-17 | 2022-05-31 | 长沙慧联智能科技有限公司 | 基于参考线匹配的车辆全局定位方法及装置 |
| CN114563006B (zh) * | 2022-03-17 | 2024-03-19 | 长沙慧联智能科技有限公司 | 基于参考线匹配的车辆全局定位方法及装置 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110631593B (zh) | 一种用于自动驾驶场景的多传感器融合定位方法 | |
| CN109916431B (zh) | 一种针对四轮移动机器人的车轮编码器标定算法 | |
| CN104061899B (zh) | 一种基于卡尔曼滤波的车辆侧倾角与俯仰角估计方法 | |
| CN102928816B (zh) | 一种面向隧道环境的车辆高可靠组合定位方法 | |
| CN100504694C (zh) | 一种农业机械的导航控制方法 | |
| CN108362288B (zh) | 一种基于无迹卡尔曼滤波的偏振光slam方法 | |
| CN110702091B (zh) | 一种沿地铁轨道移动机器人的高精度定位方法 | |
| Chi et al. | Automatic guidance of underground mining vehicles using laser sensors | |
| Anousaki et al. | A dead-reckoning scheme for skid-steered vehicles in outdoor environments | |
| CN104019828A (zh) | 高动态环境下惯性导航系统杆臂效应误差在线标定方法 | |
| CN103914068A (zh) | 一种基于栅格地图的服务机器人自主导航方法 | |
| CN111426320B (zh) | 一种基于图像匹配/惯导/里程计的车辆自主导航方法 | |
| Pfaff et al. | Towards mapping of cities | |
| CN106843214A (zh) | 一种基于自抗扰控制的磁带导引agv循迹控制方法 | |
| CN110763224A (zh) | 一种自动导引运输车导航方法及导航系统 | |
| CN107219542B (zh) | 基于gnss/odo的机器人双轮差速定位方法 | |
| CN110160557B (zh) | 一种掘进机惯性导航系统二维位置精度标定方法和系统 | |
| CN112904317A (zh) | 一种多激光雷达与gnss_ins系统标定方法 | |
| CN114966629A (zh) | 一种基于ekf算法框架的车体激光雷达外参标定方法 | |
| CN116337045A (zh) | 一种基于karto和teb的高速建图导航方法 | |
| Fu et al. | An improved integrated navigation method based on RINS, GNSS and kinematics for port heavy-duty AGV | |
| CN112129297B (zh) | 一种多传感器信息融合的自适应校正室内定位方法 | |
| CN111256708A (zh) | 一种基于射频识别的车载组合导航方法 | |
| CN112129297A (zh) | 一种多传感器信息融合的自适应校正室内定位方法 | |
| CN116338719A (zh) | 基于b样条函数的激光雷达-惯性-车辆融合定位方法 |
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 |