一种基于多传感器信息即时融合定位方法

1.本发明涉及一种基于ukf(unscentedkalman filter，无迹卡尔曼滤波)的gps、惯性测量单元和3d激光雷达多传感器信息即时融合定位方法，属于机器人感知与导航领域。


背景技术：

2.多传感器信息融合定位技术是当前机器人和无人驾驶领域具有研究前景的技术领域。多传感器信息融合定位技术克服了单一传感器存在的信息量少、及时性和鲁棒性差等问题，从而有效的提高了移动机器人在复杂环境下定位感知的准确性和可行性。
3.多传感器信息融合定位技术目前主要融合对象为视觉或激光slam(simultaneous localization andmapping，即时定位与建图)发布的定位信息、imu(inertialmeasurementunit，惯性测量单元)与gps(globalpositioning system，全球定位系统)，融合手段主要为基于滤波的ukf，以及基于优化的位姿图优化等算法。多传感器信息融合定位技术有效避免了单一传感器定位技术的缺陷，面对复杂多变的环境具有较强的适应能力，并持续能够提供精确度高、实时性好的定位信息。但是多传感器信息融合定位技术仍然存在一系列问题，如何更有效的融合传感器的定位信息使其适应各种复杂环境如室内外多场景定位、未知环境下定位方案的选取和切换仍然是专家学者们研究的热点。
4.针对有效融合多传感器信息融合定位的问题，已有的解决方案有如下几种：
5.方案1：文献(张少将.基于多传感器信息融合的智能车定位导航系统研究[d].哈尔滨工业大学,2020.)提出了一种在户外应用自适应卡尔滤波的方法融合gps、imu与激光slam的定位信息，在室内采用非线性紧耦合优化的方法融合了视觉slam与激光slam的算法框架，从而实现了室内外多场景的即时定位。但是算法没有实现室内外的灵活切换同时也没有给出传感器信息来源的置信度，从而无法判断传感器信息是否精准可靠。
[0006]
方案2：文献(demirm,fujimurak.robust localization with low-mounted multiple lidars in urban environments[c]//2019ieee intelligent transportation systems conference-itsc.ieee,2019.)提出了一种采用扩展卡尔曼滤波算法融合gps、imu、视觉和激光slam的室外定位框架，采用基于ndt(normal distributions transform，正态分布变换)的激光定位方法，结合动态障碍物识别算法提出一种计算激光定位置信度的方案，从而有效的弥补了激光定位信息置信度不确定的缺陷，进而提高了整体框架的定位精度。但美中不足的是，文中没有计算gps的定位置信度，以致无法判断gps提供的定位信息是否准确。
[0007]
本发明基于上述两种方案的启发，根据所要解决问题的本身特点，加以实验验证，提出一种基于ukf的gps、imu和3d激光雷达多传感器信息即时融合定位方法，在点云地图已知的情况下，依据差分gps所给出的定位信息初始化基于ndt的激光雷达定位算法，并同时通过ukf算法融合gps、激光定位算法与imu提供的位姿信息并实时计算激光定位算法的定位置信度以及应用lo-ransac(locally optimized random sample consensus，随机抽样一致算法)算法实时计算与gps定位信息的匹配程度，从而克服了差分gps易被遮挡，激光雷
达定位算法及时性差，imu位姿信息精度低等缺陷，进而能够在室内外等复杂环境中即时去除置信度较低的传感器信息，保留置信度较高的传感器信息，从而保持融合后定位的准确性与实时性。


技术实现要素：

[0008]
本发明针对室内外多场景定位算法选取与切换问题，提出了一种基于多传感器信息即时融合定位方法，解决了单一传感器鲁棒性差、精确度低、实时性差的问题。
[0009]
本发明通过以下技术方案实现。
[0010]
一种基于多传感器信息的即时融合定位方法，包括：
[0011]
步骤一、通过雷达利用ndt方法确定无人车平台的当前位置与姿态，并采用欧式距离残差的方法求解定位置信度；
[0012]
步骤二、将gps和组合惯导发布的原始定位信息与步骤一得到的当前位置与姿态进行匹配，采用基于滑动窗口和lo-ransac方法进行gps定位信息的置信度估计，得到经过时间戳对齐的激光雷达和gps定位信息配对点集；
[0013]
步骤三、将步骤一得到的当前位置与姿态和定位置信度以及步骤二得到的激光雷达和gps定位信息配对点集结合imu位姿信息采用ukf方法进行融合，得到无人车平台实时高精度定位结果。
[0014]
本发明的有益效果：
[0015]
本发明通过激光雷达确定无人车平台的当前位置与姿态并计算其定位置信度并将激光雷达与gps原始定位信息进行匹配计算得到gps定位信息置信度估计。依据上述所得的激光雷达与gps定位信息与置信度，赋予激光雷达和gps定位信息合适的协方差矩阵，并加入imu获取的姿态信息进行ukf融合，最终得到由激光雷达、gps定位信息与imu姿态信息的无人车平台实时高精度定位结果；在室内外多环境、未知环境等复杂条件下依然持续发布高精度的定位信息，同时采用的lo-ransac方法较传统ransac方法精确度更高、计算时间更少，并在实际测试中具有较好的鲁棒性、实时性和精确性。
附图说明
[0016]
图1为本发明的基于多传感器信息即时融合定位方法流程图。
具体实施方式
[0017]
下面结合附图对本发明做详细说明。
[0018]
如图1所示，本发明具体实施方式的一种基于多传感器信息即时融合定位方法，具体包括以下步骤：
[0019]
步骤一、通过雷达利用ndt(normal distributions transform,正态分布变换)方法确定无人车平台的当前位置与姿态，并采用欧式距离残差的方法求解定位置信度；具体实施时，本步骤在点云地图和初始坐标与姿态已知的条件下进行，所述雷达采用激光雷达；具体如下：
[0020]
1.1通过下采样方式将当前雷达扫描得到的点云转换为稀疏的局部点云，并利用ndt方法将所述局部点云和对应的原始点云地图转换为二维栅格地图的概率分布qi和pi；
[0021]
1.2将上一时刻计算得到的姿态增量以及对imu位姿数据进行预积分得到当前位姿的预测值，将所述概率分布根据所述预测值粗匹配至世界坐标系，得到粗略的平移旋转矩阵t
ini
；
[0022]
1.3通过构建优化目标匹配当前局部点云与输入的原始点云地图，利用梯度下降法最小化的优化方程，以所述粗略的旋转平移矩阵t
ini
作为初始值，得到当前无人车位姿与上一时刻位姿间的旋转平移矩阵从而得到当前无人车平台的位姿信息，实现对无人车平台当前时刻位姿精准的定位；具体为：
[0023][0024]
其中，pi为原始点云地图中点经过ndt变换得到的二维栅格地图的概率分布，qi为与之对应的雷达扫描点的ndt变换后的二维栅格地图的概率分布，为最终得到的旋转平移矩阵；
[0025]
1.4根据所述旋转平移矩阵计算得到当前雷达扫描点与原始点云地图对应点之间的欧式距离残差和。具体计算公式如下所示：
[0026][0027]
其中，pi为原始点云地图中点的二维坐标，qi为与之对应的雷达局部扫描点的二维坐标，score为欧式距离残差和，即激光雷达定位置信度估计。
[0028]
这一步骤的实现原理是，通过对所述欧式距离残差和进行评估，进而对激光雷达定位置信度进行估计，从而对激光雷达定位算法发布的定位里程计信息赋予合适的协方差矩阵。
[0029]
步骤二、将gps和组合惯导发布的原始定位信息与步骤一得到的当前位置与姿态进行匹配，采用基于滑动窗口和lo-ransac方法进行gps定位信息的置信度估计，得到经过时间戳对齐的激光雷达和gps定位信息配对点集；
[0030]
本实施例中，所述采用基于滑动窗口和lo-ransac方法进行gps定位信息的置信度估计，具体步骤如下：
[0031]
2.1通过初始化滑动窗口得到适当长度的滑动窗口和窗口内一组时间戳较为匹配初始的激光雷达和gps定位信息点集，
[0032]
2.2利用基于时间戳的方法匹配输入的激光雷达定位信息和gps定位信息，同时选择前一次lo-ransac计算时间作为滑动窗口的前进速度(本实施例中约1.0s)；
[0033]
这两步骤通过激光雷达和gps信息的读取与预处理提高了lo-ransac匹配检测结果的实时性和精确度，利用了基于多线程和滑动窗口的数据读取与预处理机制，解决了激光雷达和gps定位信息在发布、传输和读取过程中存在不同程度的时延导致匹配结果实时性差精确度低的问题。
[0034]
2.3采用lo-ransac算法匹配所述激光雷达定位信息和gps定位信息，估计gps定位置信度；具体步骤如下：
[0035]
2.3.1根据总点集中选择最小样本点数，利用随机选择方法得到一个最小样本点数的最小点集sm；
[0036]
2.3.2利用icp(iterative closest point，迭代最近点)方法估计符合所述最小点集sm的模型，所述icp方法的匹配公式如下：
[0037][0038]
其中，qi为第i个激光雷达定位信息，pi为第i个gps定位信息，r为旋转矩阵，t为平移向量；
[0039]
2.3.3将所述模型推广至总点集，计算在预设阈值(本实施例中)内符合该模型的点集数量，即内点数量；
[0040]
2.3.4比较设定的比例常数和所述内点数量实际占总点数比例，从而判断所述模型好坏；
[0041]
本实施例中，采用以下方式：若内点所占比例大于预设比例，则通过局部优化算法得到精确的内点数和模型，即将当前内点集作为输入点集，重复执行步骤2.3.1-2.3.3指定次数，同时更新运行次数上限k的方法进行优化；若不满足上述条件，则重复步骤2.3.1-2.3.3。所述运行次数上限k的更新规则如下所示：
[0042][0043]
其中，p为默认置信度，w为内点所占比例，m为输入的指定运行次数。
[0044]
2.3.5比较运行次数和所述运行次数上限，当达到所述运行次数上限k，则将存储的运行结果取平均值并输出内点占输入点集的元素的比例，即内点率；
[0045]
2.3.6将所述内点率赋予gps定位信息的协方差矩阵，判断gps定位置信度。
[0046]
这一步骤解决了在室内外切换环境下gps定位置信度波动以及组合惯导未输出协方差矩阵的情况下对gps定位置信度的估计的问题。
[0047]
步骤三、将步骤一得到的当前位置与姿态和定位置信度以及步骤二得到的激光雷达和gps定位信息配对点集结合imu位姿信息采用ukf进行融合，得到无人车平台实时高精度定位结果。具体步骤如下：
[0048]
3.1将步骤一得到的当前位置与姿态和定位置信度结合imu位姿信息作为观测变量对无人车平台进行建模，得到运动方程和观测方程：
[0049]
x
t
＝g(x
t-1
)
[0050]zt
＝h(x
t
)
[0051]
其中，为t时刻无人车的状态变量，其中，x，y，z为三维坐标，roll，pitch，raw为欧拉角，为线速度，为角速度，为线加速度；z
t
为观测变量，其中激光雷达和gps定位信息中位置和姿态的融合变量为二维坐标x，y以及欧拉角roll，pitch，raw，imu位姿信息的融合变量为角速度g和h为状态方程和观测方程的变换函数，本实施例中运动方程为短时间的均速直线运动模型，观测方程为单位阵乘以对应的观测变量。
[0052]
3.2根据所述运动方程和观测方程计算状态变量的加权均值和方差的，再利用t-1时刻的所述均值μ
t-1
和方差∑
t-1
对t-1时刻状态变量进行采样得到σ点，利用非线性变换得
到t时刻状态变量并选择权重，结合测量误差计算得到预测的观测变量均值和方差；具体公式如下所示：
[0053][0054][0055][0056][0057]
其中，μ
t-1
为t-1时刻的均值，∑
t-1
为t-1时刻的方差，γ为比例因子，为无噪声的预测的t时刻无人车的状态变量，为预测的t时刻无人车的状态变量的加权均值，为预测的t时刻无人车的状态变量的方差，为选取的权值，r
t
为测量误差。
[0058]
3.3根据所述预测的观测变量均值和方差采样得到σ点，并对得到的σ点进行非线性变换得到预测的观测变量，结合过程噪声计算预测观测变量均值和方差，如下所示：
[0059][0060][0061][0062][0063]
其中，为加入了预测阶段不确定性的新σ点，为预测的观测变量，为预测的观测变量的加权均值，为所选取的权值，s
t
为预测的观测变量的方差，q
t
为过程噪声。
[0064]
3.4利用所述预测的状态变量和观测变量以及所述加权均值和方差计算得到卡尔曼增益kt，计算过程如下所示：
[0065][0066][0067]
通过上一步骤得到的卡尔曼增益k
t
得到当前时刻t的无人车平台的定位信息，利用卡尔曼增益k
t
计算当前时刻t无人车平台的状态变量x
t
、均值μ
t
和方差∑
t
，进而得到当前时刻t无人车平台的定位信息，具体计算过程如下所示：
[0068]
[0069][0070][0071]
这一步骤通过对前两步骤得到的gps、3d激光雷达的定位里程计信息和输入的imu发布的位姿信息进行融合得到了鲁棒性高、实时性好且精确度高的融合定位里程计信息，利用了基于ukf的滤波融合方式进行多传感器融合定位，解决了在室内外切换环境和未知环境中，差分gps易被遮挡，激光雷达定位算法实时性差，imu位姿信息精度低等缺陷的问题。