CN115900708A - 基于gps引导式粒子滤波的机器人多传感器融合定位方法 - Google Patents
基于gps引导式粒子滤波的机器人多传感器融合定位方法 Download PDFInfo
- Publication number
- CN115900708A CN115900708A CN202211263413.XA CN202211263413A CN115900708A CN 115900708 A CN115900708 A CN 115900708A CN 202211263413 A CN202211263413 A CN 202211263413A CN 115900708 A CN115900708 A CN 115900708A
- Authority
- CN
- China
- Prior art keywords
- gps
- particles
- particle
- positioning
- data
- 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.)
- Pending
Links
- 239000002245 particle Substances 0.000 title claims abstract description 325
- 238000000034 method Methods 0.000 title claims abstract description 53
- 230000004927 fusion Effects 0.000 title claims abstract description 39
- 238000005259 measurement Methods 0.000 claims abstract description 76
- 238000001914 filtration Methods 0.000 claims abstract description 31
- 238000012952 Resampling Methods 0.000 claims abstract description 19
- 238000005070 sampling Methods 0.000 claims abstract description 14
- 230000033001 locomotion Effects 0.000 claims abstract description 9
- 239000011159 matrix material Substances 0.000 claims description 15
- 230000001133 acceleration Effects 0.000 claims description 6
- 239000000126 substance Substances 0.000 claims description 6
- 238000007781 pre-processing Methods 0.000 claims description 5
- 150000001875 compounds Chemical class 0.000 claims description 4
- 230000003068 static effect Effects 0.000 claims description 4
- 230000001131 transforming effect Effects 0.000 claims description 2
- 239000008187 granular material Substances 0.000 claims 1
- 238000004364 calculation method Methods 0.000 abstract description 6
- 230000008901 benefit Effects 0.000 description 10
- 230000000694 effects Effects 0.000 description 8
- 230000007613 environmental effect Effects 0.000 description 6
- 238000010586 diagram Methods 0.000 description 4
- 230000004807 localization Effects 0.000 description 4
- 230000002411 adverse Effects 0.000 description 3
- 238000005457 optimization Methods 0.000 description 3
- 238000007500 overflow downdraw method Methods 0.000 description 3
- 230000008569 process Effects 0.000 description 3
- 230000015556 catabolic process Effects 0.000 description 2
- 230000007547 defect Effects 0.000 description 2
- 238000006731 degradation reaction Methods 0.000 description 2
- 238000001514 detection method Methods 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 238000009434 installation Methods 0.000 description 2
- 238000013507 mapping Methods 0.000 description 2
- 230000009466 transformation Effects 0.000 description 2
- 230000000007 visual effect Effects 0.000 description 2
- NAWXUBYGYWOOIX-SFHVURJKSA-N (2s)-2-[[4-[2-(2,4-diaminoquinazolin-6-yl)ethyl]benzoyl]amino]-4-methylidenepentanedioic acid Chemical compound C1=CC2=NC(N)=NC(N)=C2C=C1CCC1=CC=C(C(=O)N[C@@H](CC(=C)C(O)=O)C(O)=O)C=C1 NAWXUBYGYWOOIX-SFHVURJKSA-N 0.000 description 1
- 102100025026 E3 ubiquitin-protein ligase TRIM68 Human genes 0.000 description 1
- 101100370400 Homo sapiens TRIM68 gene Proteins 0.000 description 1
- 230000003044 adaptive effect Effects 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 230000000295 complement effect Effects 0.000 description 1
- 238000011156 evaluation Methods 0.000 description 1
- 238000009499 grossing Methods 0.000 description 1
- 238000007689 inspection Methods 0.000 description 1
- 230000010354 integration Effects 0.000 description 1
- 230000007704 transition Effects 0.000 description 1
- 238000013519 translation Methods 0.000 description 1
Images
Landscapes
- Position Fixing By Use Of Radio Waves (AREA)
Abstract
本发明涉及一种基于GPS引导式粒子滤波的机器人多传感器融合定位方法，在3D点云地图中基于GPS导引式粒子滤波，实现差分GPS、激光雷达和IMU的融合。判断有效GPS粒子数量的方式决定是否在GPS量测值附近添加少量新粒子，降低定位方法对GPS信号的依赖程度，GPS仅在信号好时发挥引导粒子运动方向的作用。利用激光雷达或IMU的量测数据计算粒子权值，而不采用与GPS加权的方式，避免了GPS生成的粒子会在GPS量测下得到较高的权值。当GPS定位数据不可用时，加入的GPS粒子在激光量测下将得到较低的权值，同时为了避免添加GPS粒子带来的计算量的问题，在重采样步骤前对粒子进行随机降采样，确保粒子数量始终维持在设定范围内，保证了定位系统的实时性。
Description
技术领域
本发明属于移动机器人室外定位领域，涉及一种基于GPS引导式粒子滤波的机器人多传感器融合定位方法。
背景技术
移动机器人在室外复杂环境下的高精度定位是实现移动机器人包括巡检、安防以及更多高级功能的先验基础之一。定位的准确性和稳定性直接决定了机器人是否可以安全、稳定地完成自主导航任务，而自主导航功能又是机器人实现其预设高级功能的基础。
在室外环境中，当前主流的定位方法依据是否使用预先建立的环境地图可分为基于地图的定位方式和不基于地图的定位方式。不基于地图的定位方式依靠传感器自身的精度，难以利用环境的纹理特征，不利于机器人后续的路径规划；基于地图的定位方式又可细分为单传感器定位、多传感器融合定位两大类，其中依靠单一传感器的常用定位方法包括：GPS定位、激光雷达定位、视觉定位、惯性导航定位技术。
GPS定位是一种以人造地球卫星为基础的高精度无线电导航的定位方式，具备在室外无遮蔽场所下进行全方位实时定位的能力，但是当环境中存在较多遮蔽物时，会由于多径效应导致定位误差相对偏大，不适用于精度需求高的场景。激光雷达定位的方式有两种，一种是基于地图匹配的定位方法，利用预先构建的环境地图与激光雷达获得的量测数据进行匹配，获得机器人在环境中的定位结果，该定位方式的精度取决于环境的纹理特征是否丰富。另一种方式是利用帧间点云数据变化，进行航迹推算，获取机器人的定位结果。视觉定位则是通过相机帧间图像数据的匹配实现机器人的位置估计，但该方式受环境光线影响较大。惯性导航定位是利用惯性元器件测量到的机器人自身的姿态、速度、加速度等参数，结合相关算法推算出机器人自身的位置。该方法只能获取到短时间内的高精度定位，无法长时间提供稳定的高精度定位，难以在室外大环境下长期使用。
因此在复杂室外场景下，依靠单一传感器难以提供稳定的高精度定位，多传感器紧密融合定位是当前移动机器人室外定位的主流趋势，而GPS和激光雷达也因为两者具备互补的优势引起广泛的关注，GPS在开阔场景下定位精度较高，在建筑物密集的场景精度低，而激光雷达定位是依靠将自身测量结果与环境地图匹配获得的，其在环境特征丰富、建筑物密集的场景定位精度高，GPS则与之相反。研究者通常基于贝叶斯概率理论，利用卡尔曼滤波、粒子滤波等算法实现多传感器数据融合。粒子滤波由于其在非线性、非高斯系统的优越性，被广泛应用于移动机器人的自主定位。
在现有的融合定位技术中，文献(Perea-Strom D,Morell A,Toledo J,etal.GNSS Integration in the Localization System of an Autonomous Vehicle Basedon Particle Weighting[J].IEEE Sensors Journal,2019,PP(99):1-1.)基于粒子滤波框架实现激光雷达与GPS传感器的融合定位，其分别获取粒子在激光雷达和GPS测量数据下的概率分布，采用概率分布的乘积对粒子的权值进行更新，不生成新粒子，该融合定位方式当其中某一传感器定位数据较差或不可用时，将会影响整体的定位效果，无法保证定位的精度与鲁棒性。
文献(Miguel M,F García,Armingol J M.Improved LiDAR ProbabilisticLocalization for Autonomous Vehicles Using GNSS[J].Sensors(Basel,Switzerland),2020,20(11).)中基于自适应蒙特卡洛定位(AMCL)框架实现激光雷达与GPS传感器的融合定位，其分别计算粒子在激光雷达与GPS量测下的权值，采用加权的方式对两种权值进行组合，最终根据粒子在GPS量测值下权值的均值判别是否生成GPS粒子。该方法的定位精度依赖于算法对激光雷达数据与GPS数据所分配的权值大小，同时生成的GPS粒子会在GPS的量测中得到较大的权值，导致定位精度受GPS信号的影响较大，定位鲁棒性较差。
经过较为全面的检索，发明人发现现有的室外融合定位技术大都存在如下问题：其一，定位方法多在2D栅格地图中实现，在室外复杂环境中通过投影的方式将3D点云地图转换为2D栅格地图，损失了大量环境信息，不利于激光雷达数据与环境地图的匹配，影响定位精度；其二，定位方式对GPS信号的依赖度较高，融合GPS数据时未充分考虑GPS信号不好或完全不可用时对定位精度的影响，定位鲁棒性差。其三，地图构建时不可避免的建图误差会导致环境地图与GPS定位数据匹配有差，GPS数据直接映射到地图中会造成较大的定位误差。
发明内容
要解决的技术问题
为了避免现有技术的不足之处，本发明提出一种基于GPS引导式粒子滤波的机器人多传感器融合定位方法，为实现机器人在室外复杂场景下的自主导航，提供了一种在3D点云地图中用于多传感器融合定位的GPS引导式粒子滤波算法。该算法基于粒子滤波框架实现了多传感器的数据融合，综合了各个传感器的优点，具备定位精度高、鲁棒性强的特点。
技术方案
本发明中，涉及的专业词解释：
IMU(Inertial Measurement Unit，惯性测量单元):测量物体三轴姿态角(或角速率)以及加速度的装置；
SLAM(Simultaneous Localization and Mapping,即时定位与地图构建):机器人在未知环境中从一个未知位置开始移动,在移动过程中根据位置和地图进行自身定位，同时在自身定位的基础上建造增量式地图，实现机器人的自主定位和导航；
UTM(Universal Transverse Mercator Grid System，通用横墨卡托格网系统)坐标是一种平面直角坐标,这种坐标格网系统及其所依据的投影已经广泛用于地形图，作为卫星影像和自然资源数据库的参考格网以及要求精确定位的其他应用。
一种基于GPS引导式粒子滤波的机器人多传感器融合定位方法，其特征在于步骤如下：
步骤1：对传感器数据进行预处理，即将IMU数据进行平滑滤波，将差分GPS获取的经纬度坐标以及偏航角转化为UTM坐标系下；
步骤2：遥控机器人运动，记录起点处GPS的定位信息
构造GPS里程计，并在起点处建立全局点云地图坐标系Map系与GPS东北天坐标系gps系；将预处理后的IMU数据、激光雷达数据和GPS里程计数据作为3D SLAM算法的输入，输出为全局点云地图，并保存全局点云地图为pcd格式，此地图是3D点云地图，无需进行2D投影；
步骤3：计算机器人在GPS东北天坐标系gps系下的定位数据:
式中，
为k时刻GPS在UTM坐标系下的定位数据；
将机器人在GPS东北天坐标系gps系中的位置信息变换到全局点云地图坐标系Map系下：
计算GPS数据变换到全局点云地图坐标系下的定位数据，并发布该定位数据，后续步骤用到的GPS定位数据均为该数据：
步骤4：加载构建好的全局点云地图，利用激光雷达、IMU和GPS的定位数据基于粒子滤波框架实现融合定位，具体步骤如下：
步骤(1)初始化粒子
步骤(2)对每个粒子的状态进行预测
步骤(3)通过获取的激光雷达数据与IMU数据计算每个粒子的权值
对于激光雷达数据，利用体素降采样过滤点云数据，将过滤后的点云数据利用正态分布变换法NDT与3D点云地图中每个粒子位姿处的点云匹配，并利用得分函数h1(·)计算一次迭代配准的得分，计算粒子权值：
其中，
为上一时刻粒子权值，
为测量概率，zlaser指当前时刻经过体素降采样后的激光雷达数据，h1(·)指激光雷达数据通过NDT一次迭代配准的得分函数，
指粒子状态的预测值；
对于IMU数据，计算粒子权值：
其中，
为上一时刻粒子权值，
为测量概率，
指粒子状态的预测值，zimu是当前时刻的IMU的量测数据，h2(·)是IMU的观测方程，Qk+1是IMU观测协方差矩阵；
所述IMU的观测方程h2(·)：
其中，δk+1是IMU的观测噪声，δk+1的协方差为Qk+1，Qk+1用于衡量当前时刻IMU测量误差的大小；
步骤(4)对每个粒子的权值进行归一化，获得每个粒子归一化后的权值
步骤(5)对粒子进行重采样，复制权值较高的粒子，去除权值较低的粒子，获取机器人位置的真实概率分布；
步骤(6)发布当前时刻机器人的定位结果Φk+1：
步骤(7)添加GPS粒子，执行完转步骤(2)进行下一次预测，直至结束；
经过重采样后的粒子在GPS量测下的权值
其中g(·)为GPS的观测方程：
zk+1指当前时刻GPS的量测数据，Σk+1指GPS测量噪声的协方差矩阵，
指当前时刻第i个粒子的状态；
定义在GPS量测数据下，权值
的粒子称之有效GPS粒子，计算有效GPS粒子数占总粒子数的百分比Ng：
判断：当有效GPS粒子数占比Ng小于阈值Nth时，向粒子群中添加m个粒子；
当有效GPS粒子数占比Ng大于阈值Nth时，目前粒子的位姿与GPS的定位数据接近，不需要添加新粒子，返回(2)进行下一步预测；
所述m个粒子服从N(μk+1,Σk+1)的正态分布，μk+1＝(xk+1,yk+1,θk+1)和Σk+1分别是当前时刻GPS定位结果和定位协方差矩阵，从正态分布N(μk+1,Σk+1)中随机生成该m个粒子：
m＝[(Nth-Ng)·n]，
其中：[·]为取整函数，n为粒子群中粒子总数。
所述全局点云地图坐标系Map系与GPS东北天坐标系gps系均为静止坐标系。
所述全局点云地图坐标系Map系为右手坐标系，x轴与机器人起点处的朝向保持一致，z轴向上。
所述GPS东北天坐标系gps系x轴指向正东，y轴指向正北，z轴向上。
所述步骤(1)初始化粒子：初始时刻，k＝0，利用GPS的定位数据初始化粒子的分布；GPS定位数据提供的初始位置Φ0服从N(μ0,Σ0)的正态分布，其中,μ0＝(x0,y0,θ0)和Σ0分别是GPS的初始定位结果和定位协方差矩阵；依据
从该分布中采样，每一个粒子的状态
由向量
表示，其中，
是初始时刻第i个粒子在空间中的位置，
和
分别是初始时刻第i个粒子的偏航角、线速度、角速度和线加速度。
所述步骤(2)对每个粒子的状态进行预测：将每个粒子的状态
代入到系统状态预测方程
获取下一时刻粒子状态预测值
实现对每一个粒子
状态的预测。
所述步骤(5)对粒子进行重采样，复制权值较高的粒子，去除权值较低的粒子，获取机器人位置的真实概率分布：
步骤1)统计粒子总个数n，当n大于粒子总个数的阈值上限nth时，对粒子数进行随机降采样，减小粒子总个数；
步骤2)根据每个粒子的权值
计算有效粒子数Neff，设Nlow为有效粒子数的下限阈值；当有效粒子数Neff大于Nlow，执行下一步；当有效粒子数Neff低于Nlow时，进行重采样，复制权值较高的粒子，淘汰权值较低的粒子，再将所有粒子的权值设为1/n，n为粒子总数：
所述nth选取粒子总数n的1.1～1.2倍。
所述Nth取1％～6％之间。
所述有效粒子数的下限阈值Nlow设置为粒子总个数n的0.5倍。
有益效果
本发明提出的一种基于GPS引导式粒子滤波的机器人多传感器融合定位方法，在3D点云地图中基于GPS导引式粒子滤波的多传感器融合定位方法，以实现差分GPS、激光雷达和IMU的融合。该方法直接加载已经构建好的3D点云地图，不需要转换2D栅格地图，保证了地图信息不丢失，为定位精度的提高提供了先决条件。通过判断有效GPS粒子数量的方式决定是否在GPS量测值附近添加少量新粒子，降低定位方法对GPS信号的依赖程度，GPS仅在信号好时发挥引导粒子运动方向的作用。利用激光雷达或IMU的量测数据计算粒子权值，而不采用与GPS加权的方式，避免了GPS生成的粒子会在GPS量测下得到较高的权值，同时避免了GPS信号不好的数据对整体定位效果的影响。其中激光雷达的量测方式通过NDT(正态分布转换)算法将当前点云数据与已知点云地图进行匹配，利用配准得分对粒子进行评价，且该算法具有速度较快，对初值不敏感等优点。当GPS定位数据与环境地图匹配误差较大或GPS信号较差甚至完全不可用时，加入的GPS粒子在激光量测下将得到较低的权值，该部分粒子将会通过重采样被淘汰掉，不会影响整体的定位精度，提高了定位系统的鲁棒性。同时为了避免添加GPS粒子带来的计算量的问题，在重采样步骤前对粒子进行随机降采样，确保粒子数量始终维持在设定范围内，保证了定位系统的实时性。
由于步骤三中利用旋转变换关系，将GPS的定位数据变换到全局点云地图坐标系下，使得GPS定位数据可以准确映射到全局点云地图中，减小了全局点云地图与GPS定位数据匹配误差对机器人定位和路径规划带来的负面影响。
由于步骤四加载的全局点云地图为3D点云地图，后续定位也是在3D点云地图中完成的，避免了通过重投影转2D地图过程中损失的环境信息对定位精度造成的影响。
由于步骤四中(3)利用激光雷达与IMU数据进行量测更新，避免了已有融合方法中采用GPS量测数据与激光量测数据加权计算的方式，以牺牲定位精度来获取稳定定位的弊端，充分发掘了激光雷达数据与全局点云地图的匹配能力以及各传感器的定位优势。实现了机器人在室外复杂环境中高精度、高鲁棒性的定位。
由于步骤四中(6)利用计算有效GPS粒子数占总粒子数的百分比决定是否加入少量GPS粒子，在空旷环境中激光雷达的定位精度差，通过添加少量分布在GPS量测附近的粒子可以为整体粒子的探索提供导向，而添加的GPS粒子在激光的量测下得到较高的权值，粒子会逐步过渡到GPS量测值附近的最优位置。解决了空旷环境中激光雷达定位差的效果。
由于步骤四中(5)重采样前对粒子进行随机降采样，确保粒子数量始终维持在设定范围内，避免了添加GPS粒子带来的计算量问题，保证了定位系统的实时性。
由于步骤四采用了基于GPS引导式粒子滤波的多传感器融合定位框架，通过较小比例分布在GPS量测值附近的粒子来引导粒子滤波量测更新方向，利用激光雷达与IMU的数据进行量测更新，在环境特征明显建筑物密集的环境中，GPS定位精度低于激光雷达，以激光雷达数据评价粒子权值和重采样可以最大程度发挥激光雷达的定位优势，使得大部分粒子分布在激光雷达量测值附近，而添加的少量分布在GPS量测附近的粒子不会对定位产生不利影响；在空旷环境中，GPS的精度优于激光雷达，少量分布在GPS量测值附近的粒子可以为粒子的探测搜索提供导向，在激光雷达的量测下，分布在GPS量测值附近的粒子权值增加，粒子会逐步过渡到GPS量测值附近的一个最优位置，避免由于粒子退化而出现陷入局部最优的情形。避免了粒子滤波器在空旷场景下定位精度低、易陷入局部最优的问题。充分发掘了各个传感器的定位优势，以GPS数据为导引、激光雷达定位数据作为粒子更新的评判标准，可以实现移动机器人室外复杂环境中高精度、高鲁棒性的定位。
附图说明
图1是本发明移动机器人与传感器安装示意图；
图2是本发明定位系统接线图；
图3是本发明GPS东北天坐标系与全局点云地图坐标系匹配示意图；
图4是本发明定位策略流程图；
具体实施方式
现结合实施例、附图对本发明作进一步描述：
一种在3D点云地图中用于多传感器融合定位的GPS引导式粒子滤波算法。该算法基于粒子滤波框架实现了多传感器的数据融合，综合了各个传感器的优点，具备定位精度高、鲁棒性强的特点。
步骤一：对传感器数据进行预处理，将IMU数据进行平滑滤波，将差分GPS的获取的经纬度坐标以及偏航角转化为UTM坐标系下。
步骤二：通过遥控机器人运动，记录起点处GPS的定位信息
构造GPS里程计，并在起点处建立全局点云地图坐标系Map系与GPS东北天坐标系gps系，均为静止坐标系。其中全局点云地图坐标系Map系为右手坐标系，x轴与机器人起点处的朝向保持一致，z轴向上；GPS东北天坐标系gps系x轴指向正东，y轴指向正北，z轴向上。将预处理后的IMU数据、激光雷达数据和GPS里程计数据作为3D SLAM算法的输入，输出为全局点云地图，并保存全局点云地图为pcd格式，此地图是3D点云地图，无需进行2D投影。
步骤三：计算机器人在GPS东北天坐标系gps系下的定位数据
式中，
为k时刻GPS在UTM坐标系下的定位数据
利用式(2)将机器人在GPS东北天坐标系gps系中的位置信息变换到全局点云地图坐标系Map系下
计算GPS数据变换到全局点云地图坐标系下的定位数据，并发布该定位数据，后续步骤用到的GPS定位数据均为该数据。
步骤四：加载构建好的全局点云地图，利用激光雷达、IMU和GPS的定位数据基于粒子滤波框架实现融合定位，具体步骤如下：
(1)初始化粒子
初始时刻，k＝0，利用GPS的定位数据初始化粒子的分布。假定GPS定位数据提供的初始位置Φ0服从N(μ0,Σ0)的正态分布，其中,μ0＝(x0,y0,θ0)和Σ0分别是GPS的初始定位结果和定位协方差矩阵。可依据公式(1)从该分布中采样，每一个粒子的状态
可由向量
表示，其中，
是初始时刻第i个粒子在空间中的位置，
和
分别是初始时刻第i个粒子的偏航角、线速度、角速度和线加速度。
(2)对每个粒子的状态进行预测。将每个粒子的状态
代入到公式(2)的系统状态预测方程(恒定转率和速度的二次运动模型，CTRV)，以获取下一时刻粒子状态预测值
实现对每一个粒子
状态的预测。
(3)通过获取的激光雷达数据与IMU数据计算每个粒子的权值
由于激光雷达和IMU的数据存在频率上的差异，因此本发明先进行判断当前时刻获取的量测数据来源，仅用当前时刻获取的量测数据和对应的观测模型进行粒子权值的计算。该权值计算方式不同于已有融合方法中采用GPS量测数据与激光量测数据加权计算的方式。
若当前时刻获取的是激光雷达数据，利用体素降采样过滤点云数据，将过滤后的点云数据利用正态分布变换法(NDT)与3D点云地图中每个粒子位姿处的点云匹配，并利用得分函数h1(·)计算一次迭代配准的得分，采用公式(3)计算粒子权值：
其中，
为上一时刻粒子权值，
为测量概率，zlaser指当前时刻经过体素降采样后的激光雷达数据，h1(·)指激光雷达数据通过NDT一次迭代配准的得分函数，
指粒子状态的预测值。
若当前时刻获取的是IMU数据，则采用公式(4)计算粒子权值：
其中，
为上一时刻粒子权值，
为测量概率
指粒子状态的预测值，zimu是当前时刻的IMU的量测数据，h2(·)是IMU的观测方程，如公式(5)所示，Qk+1是IMU观测协方差矩阵。
其中，δk+1是IMU的观测噪声，δk+1的协方差为Qk+1，Qk+1用于衡量当前时刻IMU测量误差的大小。
(4)对每个粒子的权值进行归一化，获得每个粒子归一化后的权值
(5)对粒子进行重采样，复制权值较高的粒子，去除权值较低的粒子，获取机器人位置的真实概率分布。该步骤需要分两步完成：
1)统计粒子总个数n，当n大于粒子总个数的阈值上限nth(nth一般选取设定的粒子总数n的1.1～1.2倍即可)时，对粒子数进行随机降采样，减小粒子总个数。
2)根据每个粒子的权值
计算有效粒子数Neff，设Nlow为有效粒子数的下限阈值。当有效粒子数Neff大于Nlow，执行下一步；当有效粒子数Neff低于Nlow时，进行重采样，复制权值较高的粒子，淘汰权值较低的粒子，再将所有粒子的权值设为1/n(n为粒子总数)。
(6)发布当前时刻机器人的定位结果Φk+1。
(7)添加GPS粒子，执行完转(2)进行下一次预测，直至算法结束
依照公式(9)计算经过重采样后的粒子在GPS量测下的权值
其中g(·)为GPS的观测方程，如式(9)所示。
注：zk+1指当前时刻GPS的量测数据，Σk+1指GPS测量噪声的协方差矩阵。
定义在GPS量测数据下，权值
的粒子称之有效GPS粒子(ξeff的值根据GPS的测量精度合理设置即可)。计算有效GPS粒子数占总粒子数的百分比Ng，如公式(11)所示，该判断方式不同于已有方法中通过GPS量测下粒子权值的均值来计算生成新粒子的概率，并以概率大小随机产生新粒子的方法。
当有效GPS粒子数占比Ng小于阈值Nth时(Nth建议取1％～6％之间)，向粒子群中添加m个粒子(m由式(12)计算，其中[·]为取整函数，n为粒子群中粒子总数)，这m个粒子服从N(μk+1,Σk+1)的正态分布，μk+1＝(xk+1,yk+1,θk+1)和Σk+1分别是当前时刻GPS定位结果和定位协方差矩阵协。从正态分布N(μk+1,Σk+1)中随机生成该m个粒子。
m＝[(Nth-Ng)·n] (12)
当有效GPS粒子数占比Ng大于阈值Nth时，说明目前粒子的位姿与GPS的定位数据接近，不需要添加新粒子，返回(2)进行下一步预测。
通过以上步骤，可以保证滤波器中有一定比例的粒子始终分布在GPS定位数据的附近，当机器人处在建筑密集、GPS定位精度差而激光雷达定位精度高的环境中，生成的6％以下的GPS粒子在激光雷达的量测下评分低，通过重采样后被淘汰，不会影响整体粒子的定位效果，从而保证算法能充分发掘激光雷达的定位效果；当机器人处在空旷、环境特征稀疏的环境中，GPS定位的精度将会优于激光雷达，此时，分布在GPS附近的粒子在激光雷达的量测下也会获得较高的权值，使得整体的粒子快速被导引到GPS的量测附近，获取最优的定位结果。消除了GPS信号差或完全不可用时对定位的影响，减小了GPS定位数据与环境地图匹配误差对定位造成的不利影响，保证了定位结果的精度与鲁棒性。
具体实施例：
为了使本发明的目的和技术方案及优点更加清楚明白，以下将结合附图及实例，对本发明进行进一步详细说明。显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
SS1：本发明所提及的用于实现在3D点云地图中基于导引式粒子滤波的多传感器融合定位方法的机器人如图1所示，配备十六线激光雷达、IMU和差分GPS，各个传感器安装位置和传感器坐标系朝向已在图1中给出。
SS2：对机器人运动过程中IMU和GPS原始量测数据进行预处理。
SS21：对IMU的原始测量数据进行预处理，通过调用ROS系统中的imu_tools功能包对IMU数据进行校正和平滑滤波，并将预处理后的数据进行发布。
SS22：将GPS的原始经纬度坐标转换为UTM坐标系(以正东为x轴，正北为y轴)下的坐标，转换后的GPS定位数据为zUTM＝(xUTM,yUTM,θUTM)，其中(xUTM,yUTM)为UTM坐标系下的位置，θUTM为在UTM坐标系下的偏航角。
SS3：通过远程遥控机器人移动，并订阅传感器数据，通过3D SLAM建图算法构建室外环境的全局点云地图，并保存全局点云地图。
SS31：在机器人移动前，记录起点处GPS在UTM坐标系的位姿
并在起点处构建GPS东北天坐标系(静止坐标系)gps系；
SS32：将当前k时刻GPS在UTM坐标系的位姿
与起点处的位姿
作差，构造GPS里程计，同时获取前k时刻GPS在东北天坐标系gps下的位姿
SS33：将激光雷达数据、预处理后的IMU数据和GPS里程计数据作为3D SLAM算法LIO_SAM的输入，输出室外环境的全局点云地图，并保存该全局点云地图。
SS4：在获取的全局点云地图中实现定位前，需要考虑GPS数据与全局点云地图的对齐，将GPS在东北天坐标系gps系下的位姿，转换到全局点云地图坐标系Map下。
SS41：参照图3所示的GPS的东北天坐标系gps系与全局点云地图坐标系Map匹配示意图，全局点云地图坐标系Map系为右手坐标系，x轴与机器人建图起点朝向一致，z轴朝上两个坐标系之间不存在平移关系，故利用全局点云地图坐标系Map系与GPS的东北天坐标系gps系的旋转矩阵，将gps系下的位置坐标变换到Map系
式中，
为k时刻GPS在gps系下的位置转换到Map系下的位置坐标。
SS42：计算GPS从gps系转换到Map系下的定位数据
并发布该位姿数据。
SS5：加载已经构建好的全局点云地图，参考图4所示的定位流程图，基于粒子滤波框架，融合激光雷达、IMU、GPS传感器数据实现在已知全局点云地图中的融合定位，具体步骤如下：
SS51：系统上电初始化，为运算单元和传感器供电；
SS52：订阅经过预处理的IMU数据、GPS在Map系下的位姿数据和激光雷达数据，并在ROS系统中发布；
SS53：加载已经构建好的全局点云地图；
SS54：粒子初始化，k＝0时刻，利用GPS获取的定位数据初始化粒子的分布。假定GPS传感器提供的机器人初始位置Φ0服从N(μ0,Σ0)的正态分布，其中,μ0＝(x0,y0,θ0)和Σ0分别是GPS的初始定位数据和定位协方差矩阵。可依据公式(2)从该分布中采样，每一个粒子的状态
可由向量
表示，其中，
是初始时刻第i个粒子空间中的位置，
和
分别是第i个粒子的偏航角、线速度、角速度和线加速度。
SS55：对每个粒子的状态进行预测。将每个粒子的状态
代入到公式(2)的系统状态预测方程(恒定转率和速度的二次运动模型，CTRV)，以获取下一时刻粒子状态预测值
实现对每一个粒子
状态的预测。
SS56：通过激光雷达与IMU量测数据计算每个粒子的权值，利用观测模型计算粒子权值。由于激光雷达和IMU的数据存在频率上的差异，因此本发明先进行判断当前时刻获取的量测数据来源，仅用当前时刻获取的量测数据和对应的观测模型进行粒子权值的更新。该权值更新方式不同于已有融合方法中采用GPS量测数据与激光量测数据加权更新的方式。
(1)若当前时刻获取的是激光雷达数据，首先利用体素降采样对激光雷达点云数据进行过滤，再将过滤后的点云利用正态分布变换法(NDT)与3D点云地图匹配并计算配准得分，利用公式(4)对粒子权值进行更新；
其中，
为上一时刻粒子权值，zlaser指当前时刻经过体素降采样后的激光雷达数据，h1(·)指激光雷达NDT一次迭代配准的得分函数，
指粒子状态的预测值。
(2)若当前时刻获取的是IMU数据，则采用公式(5)计算粒子权值：
其中，
为上一时刻粒子权值，
指粒子状态的预测值。h2(·)是IMU的观测方程，如公式(6)所示，Qk+1是IMU观测协方差矩阵。
其中，δk+1是IMU的观测噪声，δk+1的协方差为Qk+1，Qk+1用于衡量当前时刻IMU测量误差的大小。
SS57：将每个粒子的权值通过式(7)进行归一化处理。
SS58：先统计当前时刻粒子总数n，判断当前粒子总数是否超过设定的粒子数量阈值nth(nth一般选取设定的粒子总数n的1.1～1.2倍即可)，如果超过，对粒子数进行随机降采样，减小粒子总个数，反之，执行下一步。
SS59：粒子重采样。利用公式(8)计算有效粒子数，并判断计算出的有效粒子数是否小于阈值，如果小于设定的阈值，则进行重采样，复制权值较高的粒子，淘汰权值较低的粒子，再将所有粒子的权值设为1/n(n为粒子总数)。反之，执行下一步。
SS510：计算机器人当前时刻的定位结果
SS511：通过公式(9)计算在重采样后的粒子在GPS量测下的权值
式中，zk+1指GPS定位测量值，Σk+1指GPS测量噪声的协方差矩阵，g(·)为GPS的观测方程，如式(10)所示。
SS512：将每个粒子计算得到的权值
与设定的阈值ξeff进行比较，大于该阈值则判定为有效GPS粒子，并统计有效GPS粒子的数量，并利用公式(11)计算其占比。
SS513：判断有效GPS粒子占比是否小于6％，如果小于，则利用公式(12)计算在GPS量测值附近需要添加粒子的数量m，并在其量测值附近随机生成这些粒子。反之，则转步骤SS55继续循环，直至定位算法结束。
m＝[(Nth-Ng)·n] (24)
本发明提出一种在3D点云地图中基于GPS导引式粒子滤波的多传感器融合定位方法，以实现差分GPS、激光雷达和IMU的融合。该方法直接加载已经构建好的3D点云地图，不需要转换2D栅格地图，保证了地图信息不丢失，为定位精度的提高提供了先决条件。通过判断有效GPS粒子数量的方式决定是否在GPS量测值附近添加少量新粒子，降低定位方法对GPS信号的依赖程度，GPS仅在信号好时发挥引导粒子运动方向的作用。利用激光雷达或IMU的量测数据计算粒子权值，而不采用与GPS加权的方式，避免了GPS生成的粒子会在GPS量测下得到较高的权值，同时避免了GPS信号不好的数据对整体定位效果的影响。其中激光雷达的量测方式通过NDT(正态分布转换)算法将当前点云数据与已知点云地图进行匹配，利用配准得分对粒子进行评价，且该算法具有速度较快，对初值不敏感等优点。当GPS定位数据与环境地图匹配误差较大或GPS信号较差甚至完全不可用时，加入的GPS粒子在激光量测下将得到较低的权值，该部分粒子将会通过重采样被淘汰掉，不会影响整体的定位精度，提高了定位系统的鲁棒性。同时为了避免添加GPS粒子带来的计算量的问题，在重采样步骤前对粒子进行随机降采样，确保粒子数量始终维持在设定范围内，保证了定位系统的实时性。
通过这种方式，在环境特征明显建筑物密集的环境中，GPS定位精度低于激光雷达，以激光雷达数据计算粒子权值和重采样可以最大程度发挥激光雷达的定位优势，使得绝大多数粒子分布在激光雷达量测值附近，而添加的少量分布在GPS量测附近的粒子不会对定位产生不利影响；在空旷环境中，GPS的精度优于激光雷达，少量分布在GPS量测值附近的粒子可以为粒子的探测搜索提供导向，在激光雷达的量测下，分布在GPS量测值附近的粒子权值增加，粒子会逐步过渡到GPS量测值附近的一个最优位置，避免由于粒子退化而出现陷入局部最优的情形。该方法充分发掘了各个传感器的定位优势，以GPS数据为导引、激光雷达定位数据作为粒子更新的评判标准，可以实现移动机器人室外复杂环境中高精度、高鲁棒性的定位。
Claims (10)
1.一种基于GPS引导式粒子滤波的机器人多传感器融合定位方法，其特征在于步骤如下：
步骤1：对传感器数据进行预处理，即将IMU数据进行平滑滤波，将差分GPS获取的经纬度坐标以及偏航角转化为UTM坐标系下；
步骤2：遥控机器人运动，记录起点处GPS的定位信息
构造GPS里程计，并在起点处建立全局点云地图坐标系Map系与GPS东北天坐标系gps系；将预处理后的IMU数据、激光雷达数据和GPS里程计数据作为3D SLAM算法的输入，输出为全局点云地图，并保存全局点云地图为pcd格式，此地图是3D点云地图，无需进行2D投影；
步骤3：计算机器人在GPS东北天坐标系gps系下的定位数据:
式中，
为k时刻GPS在UTM坐标系下的定位数据；
将机器人在GPS东北天坐标系gps系中的位置信息变换到全局点云地图坐标系Map系下：
计算GPS数据变换到全局点云地图坐标系下的定位数据，并发布该定位数据，后续步骤用到的GPS定位数据均为该数据：
步骤4：加载构建好的全局点云地图，利用激光雷达、IMU和GPS的定位数据基于粒子滤波框架实现融合定位，具体步骤如下：
步骤(1)初始化粒子
步骤(2)对每个粒子的状态进行预测
步骤(3)通过获取的激光雷达数据与IMU数据计算每个粒子的权值
对于激光雷达数据，利用体素降采样过滤点云数据，将过滤后的点云数据利用正态分布变换法NDT与3D点云地图中每个粒子位姿处的点云匹配，并利用得分函数h1(·)计算一次迭代配准的得分，计算粒子权值：
其中，
为上一时刻粒子权值，
为测量概率，zlaser指当前时刻经过体素降采样后的激光雷达数据，h1(·)指激光雷达数据通过NDT一次迭代配准的得分函数，
指粒子状态的预测值；
对于IMU数据，计算粒子权值：
其中，
为上一时刻粒子权值，
为测量概率，
指粒子状态的预测值，zimu是当前时刻的IMU的量测数据，h2(·)是IMU的观测方程，Qk+1是IMU观测协方差矩阵；
所述IMU的观测方程h2(·)：
其中，δk+1是IMU的观测噪声，δk+1的协方差为Qk+1，Qk+1用于衡量当前时刻IMU测量误差的大小；
步骤(4)对每个粒子的权值进行归一化，获得每个粒子归一化后的权值
步骤(5)对粒子进行重采样，复制权值较高的粒子，去除权值较低的粒子，获取机器人位置的真实概率分布；
步骤(6)发布当前时刻机器人的定位结果Φk+1：
步骤(7)添加GPS粒子，执行完转步骤(2)进行下一次预测，直至结束；
经过重采样后的粒子在GPS量测下的权值
其中g(·)为GPS的观测方程：
zk+1指当前时刻GPS的量测数据，Σk+1指GPS测量噪声的协方差矩阵，
指当前时刻第i个粒子的状态；
定义在GPS量测数据下，权值
的粒子称之有效GPS粒子，计算有效GPS粒子数占总粒子数的百分比Ng：
判断：当有效GPS粒子数占比Ng小于阈值Nth时，向粒子群中添加m个粒子；
当有效GPS粒子数占比Ng大于阈值Nth时，目前粒子的位姿与GPS的定位数据接近，不需要添加新粒子，返回(2)进行下一步预测；
所述m个粒子服从N(μk+1,Σk+1)的正态分布，μk+1＝(xk+1,yk+1,θk+1)和Σk+1分别是当前时刻GPS定位结果和定位协方差矩阵，从正态分布N(μk+1,Σk+1)中随机生成该m个粒子：
m＝[(Nth-Ng)·n]，
其中：[·]为取整函数，n为粒子群中粒子总数。
2.根据权利要求1所述基于GPS引导式粒子滤波的机器人多传感器融合定位方法，其特征在于：所述全局点云地图坐标系Map系与GPS东北天坐标系gps系均为静止坐标系。
3.根据权利要求1所述基于GPS引导式粒子滤波的机器人多传感器融合定位方法，其特征在于：所述全局点云地图坐标系Map系为右手坐标系，x轴与机器人起点处的朝向保持一致，z轴向上。
4.根据权利要求1所述基于GPS引导式粒子滤波的机器人多传感器融合定位方法，其特征在于：所述GPS东北天坐标系gps系x轴指向正东，y轴指向正北，z轴向上。
5.根据权利要求1所述基于GPS引导式粒子滤波的机器人多传感器融合定位方法，其特征在于：所述步骤(1)初始化粒子：初始时刻，k＝0，利用GPS的定位数据初始化粒子的分布；GPS定位数据提供的初始位置Φ0服从N(μ0,Σ0)的正态分布，其中,μ0＝(x0,y0,θ0)和Σ0分别是GPS的初始定位结果和定位协方差矩阵；依据
从该分布中采样，每一个粒子的状态
由向量
表示，其中，
是初始时刻第i个粒子在空间中的位置，
和
分别是初始时刻第i个粒子的偏航角、线速度、角速度和线加速度。
6.根据权利要求1所述基于GPS引导式粒子滤波的机器人多传感器融合定位方法，其特征在于：所述步骤(2)对每个粒子的状态进行预测：将每个粒子的状态
代入到系统状态预测方程
获取下一时刻粒子状态预测值
实现对每一个粒子
状态的预测。
7.根据权利要求1所述基于GPS引导式粒子滤波的机器人多传感器融合定位方法，其特征在于：所述步骤(5)对粒子进行重采样，复制权值较高的粒子，去除权值较低的粒子，获取机器人位置的真实概率分布：
步骤1)统计粒子总个数n，当n大于粒子总个数的阈值上限nth时，对粒子数进行随机降采样，减小粒子总个数；
步骤2)根据每个粒子的权值
计算有效粒子数Neff，设Nlow为有效粒子数的下限阈值；当有效粒子数Neff大于Nlow，执行下一步；当有效粒子数Neff低于Nlow时，进行重采样，复制权值较高的粒子，淘汰权值较低的粒子，再将所有粒子的权值设为1/n，n为粒子总数：
8.根据权利要求1所述基于GPS引导式粒子滤波的机器人多传感器融合定位方法，其特征在于：所述nth选取粒子总数n的1.1～1.2倍。
9.根据权利要求1所述基于GPS引导式粒子滤波的机器人多传感器融合定位方法，其特征在于：所述Nth取1％～6％之间。
10.根据权利要求1所述基于GPS引导式粒子滤波的机器人多传感器融合定位方法，其特征在于：所述有效粒子数的下限阈值Nlow设置为粒子总个数n的0.5倍。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211263413.XA CN115900708A (zh) | 2022-10-14 | 2022-10-14 | 基于gps引导式粒子滤波的机器人多传感器融合定位方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211263413.XA CN115900708A (zh) | 2022-10-14 | 2022-10-14 | 基于gps引导式粒子滤波的机器人多传感器融合定位方法 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN115900708A true CN115900708A (zh) | 2023-04-04 |
Family
ID=86488797
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202211263413.XA Pending CN115900708A (zh) | 2022-10-14 | 2022-10-14 | 基于gps引导式粒子滤波的机器人多传感器融合定位方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115900708A (zh) |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116222588A (zh) * | 2023-05-08 | 2023-06-06 | 睿羿科技(山东)有限公司 | 一种gps与车载里程计融合的定位方法 |
| CN117406259A (zh) * | 2023-12-14 | 2024-01-16 | 江西北斗云智慧科技有限公司 | 一种基于北斗的智慧工地车辆定位方法及系统 |
-
2022
- 2022-10-14 CN CN202211263413.XA patent/CN115900708A/zh active Pending
Cited By (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116222588A (zh) * | 2023-05-08 | 2023-06-06 | 睿羿科技(山东)有限公司 | 一种gps与车载里程计融合的定位方法 |
| CN116222588B (zh) * | 2023-05-08 | 2023-08-04 | 睿羿科技(山东)有限公司 | 一种gps与车载里程计融合的定位方法 |
| CN117406259A (zh) * | 2023-12-14 | 2024-01-16 | 江西北斗云智慧科技有限公司 | 一种基于北斗的智慧工地车辆定位方法及系统 |
| CN117406259B (zh) * | 2023-12-14 | 2024-03-22 | 江西北斗云智慧科技有限公司 | 一种基于北斗的智慧工地车辆定位方法及系统 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110243358B (zh) | 多源融合的无人车室内外定位方法及系统 | |
| CN110118560B (zh) | 一种基于lstm和多传感器融合的室内定位方法 | |
| CN109991636A (zh) | 基于gps、imu以及双目视觉的地图构建方法及系统 | |
| CN115900708A (zh) | 基于gps引导式粒子滤波的机器人多传感器融合定位方法 | |
| CN111272165A (zh) | 一种基于特征点标定的智能车定位方法 | |
| CN107315171B (zh) | 一种雷达组网目标状态与系统误差联合估计算法 | |
| CN113252033B (zh) | 基于多传感器融合的定位方法、定位系统及机器人 | |
| CN109807911B (zh) | 基于gnss、uwb、imu、激光雷达、码盘的室外巡逻机器人多环境联合定位方法 | |
| CN111288989B (zh) | 一种小型无人机视觉定位方法 | |
| CN108362288B (zh) | 一种基于无迹卡尔曼滤波的偏振光slam方法 | |
| CN112697138B (zh) | 一种基于因子图优化的仿生偏振同步定位与构图的方法 | |
| Cai et al. | Mobile robot localization using gps, imu and visual odometry | |
| CN110412596A (zh) | 一种基于图像信息和激光点云的机器人定位方法 | |
| CN113033494B (zh) | 基于地理空间信息数据测绘的测绘数据采集系统 | |
| WO2022193106A1 (zh) | 一种通过惯性测量参数将gps与激光雷达融合定位的方法 | |
| WO2024027350A1 (zh) | 车辆定位方法、装置、计算机设备、存储介质 | |
| CN114459470A (zh) | 基于多传感器融合的巡检机器人定位方法 | |
| CN115574816B (zh) | 仿生视觉多源信息智能感知无人平台 | |
| CN108562289A (zh) | 连续多边几何环境中四旋翼飞行器激光雷达导航方法 | |
| CN114964212A (zh) | 面向未知空间探索的多机协同融合定位与建图方法 | |
| Liu et al. | Interacting multiple model UAV navigation algorithm based on a robust cubature Kalman filter | |
| CN114608568A (zh) | 一种基于多传感器信息即时融合定位方法 | |
| Klein et al. | LiDAR and INS fusion in periods of GPS outages for mobile laser scanning mapping systems | |
| CN117685953A (zh) | 面向多无人机协同定位的uwb与视觉融合定位方法及系统 | |
| CN110794434B (zh) | 一种位姿的确定方法、装置、设备及存储介质 |
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 |