面向复杂港口环境的多传感器融合定位方法
文献发布时间:2023-06-19 19:28:50
技术领域
本发明涉及定位技术领域,具体涉及一种面向复杂港口环境的多传感器融合定位方法。
背景技术
由于自主导航技术,特别是在GPS盲区环境下的机器人定位技术的进步,使机器人在检测任务中的应用迅速发展。虽然已经提出了许多方法来使用摄像机和激光雷达等机载传感器对机器人进行定位,但在几何退化的环境中,如隧道中,实现鲁棒定位仍然是一个具有挑战性的问题。
激光雷达获得的数据具有精度高,实时好,数据稳定等特点,且激光雷达安装方便,适合作为小型环境感知系统的探测器。LOAM(Lidar Odometry and Mapping)算法,是一种基于激光雷达的SLAM算法。然而,由于激光雷达通过扫描环境来捕获几何信息,因此在隧道等几何退化的情况下,它更有可能受到影响。
UWB是近年来新兴的一种无线通信技术。基于UWB的测距的关键优势是,宽带能够以比使用其他无线技术更高的精度测量ToA和RTT。近年来,基于UWB的空间定位技术在多个军民领域中实现了广泛应用。UWB能有效地解决室内GPS信号遮挡的问题,但不适用于全局定位。
由于场景的复杂性,单一传感器无法解决所有场景下的定位问题,往往需要多种传感器进行融合定位。激光传感器数据精度高,环境适应性强,但是无法分辨几何上连续重复的场景,与UWB传感器数据融合能够在退化环境进行定位。
传统的融合框架,如卡尔曼滤波器(包括其变体扩展卡尔曼滤波器(EKF)、无迹卡尔曼滤波器(UKF)、不变扩展卡尔曼滤波算法、不变EKF),由于技术成熟、实现简单、计算效率高而非常受欢迎。然而,在隧道中,通过EKF集成GNSS/INS的性能显著下降。
此外,现有的融合框架较为单一,仅能解决单一场景下的定位问题,如针对退化环境的定位与针对无GPS信号场景下的定位,但缺乏一个通用的融合框架,以实现在不同环境下的无缝衔接的高精度车辆定位。
发明内容
为了克服上述现有技术的不足,本发明提供一种面向复杂港口环境的多传感器融合定位方法。
为此,本发明采用以下技术方案:
一种面向复杂港口环境的多传感器融合定位方法,包括以下步骤:
S1,进行多传感器坐标系统一:构建的地图基于世界坐标系
S2,提取有GPS信号且结构丰富环境下的约束,包括以下分步骤:
S21,通过GPS接收器获取GPS信号,转换为GPS定位因子:
当车辆行驶一定距离时,GPS传感器接收一组GPS数据,在k时刻的GPS定位因子
其中
其中,
S22,利用激光雷达传感器进行SLAM定位,获取单位时间内的相对位姿因子:
基于LOAM中提出的LiDAR里程法,获得连续的LiDAR帧k和k+1之间的相对位姿测量值[Δα
其中
S3,提取无GPS信号但结构丰富环境下的约束;
S4,提取无GPS信号且结构退化环境下的约束;
S5,建立面向不同传感器的非线性融合定位框架,融合不同环境下通过不同传感器获取的所有非线性约束:
其中,当GPS有信号时,a
当处在结构丰富环境时,a
利用因子图原理求解上式,获取所有约束因子最小情况下的车辆状态X
上述步骤S1中所述变换矩阵
其中G是GPS定位结果集,L是LiDAR SLAM的定位结果集。
上述的步骤S22中,步骤S22中,协方差矩阵
其中
其中
使用如下车辆运动方程,根据前两个时刻的车辆位置预测k时刻的车辆位置:
LiDAR里程计计算出的预测位置
其中,变量σ是实际情况设置。
上述的步骤S3包括以下分步骤:
S31,同步骤S22,利用激光雷达传感器进行SLAM定位,获取单位时间内的相对位姿因子:
S32,利用UWB接收器接收路侧协同定位设备发射的信号,计算UWB接收器与路侧协同定位设备之间的距离测量因子:
UWB传感器测量从路侧协同定位设备到接收器的距离,并且UWB接收器和第j个路侧协同定位设备之间的距离表示为
其中,
上述的步骤S4包括以下分步骤:
S41,利用激光雷达传感器检测到墙面并计算其与墙面之间的距离,通过与地图中的墙面进行匹配,转换为距离测量因子:
在地图生成步骤中,手动测量W中所有墙面的平面方程,并由以下方程表示:
其中
当激光雷达传感器扫描激光雷达点云时,通过基于RANSAC方法的平面检测方法来提取平面的激光雷达点;激光雷达传感器到第i个墙面之间的距离以
在k时刻检测到l个墙面,从第i个平面探测到的距离测量系数最大达2cm,并按
S42,同S32,利用UWB接收器接收路侧协同定位设备发射的信号,计算UWB接收器与路侧协同定位设备之间的距离测量因子。
与现有技术相比,本发明具有以下有益效果:
1.本发明的融合定位方法相较于传统融合定位方法误差较小。基于因子图的数据融合将当前数据和历史数据一起建立一个因子图,基于因子图建立一个批优化代价函数,然后将所有的历史信息和当前信息一起优化。在因子图优化中,由于多次迭代,所以有效减少了线性化的误差。
2.本发明的融合定位方法能够实现在场景特征变换、环境变换情况下车辆无缝链接定位的问题,比如从结构丰富到结构退化的环境,以及有GPS信号环境到无GPS信号的环境。这样的环境变换的问题会引起GPS信号缺失下的定位问题,以及不同定位方法选择与融合的问题。
3.本发明解决了结构退化环境下的定位问题。
附图说明
图1为本发明中车辆传感器安装示意图;
图2为有GPS、结构丰富的环境定位示意图;
图3为无GPS、结构丰富的环境定位示意图;
图4为无GPS、结构退化的环境定位示意图。
具体实施方式
下面结合附图和具体实例对本发明的融合定位方法进行详细说明。
本发明的面向复杂港口环境的多传感器融合定位方法在高精度地图基础上采用路侧的协同定位设备与车载多传感器定位设备,构成面向复杂港口环境的多传感器辅助定位系统。路侧协同设备采用UWB发射装置,车载多传感器包括激光雷达传感器、GPS接收器与UWB接收器。传感器的安装图如图1所示。
本发明采用的多传感器辅助定位系统包括车载多传感器、车道级地图以及路侧协同定位设备和GPS中的一个,其中:
所述车载多传感器包括GPS接收器、激光雷达传感器与UWB接收器;所述GPS接收器用于获取GPS信号;所述激光雷达传感器用于获取激光点云;所述UWB接收器用于获取车路协同定位装置发送的UWB信号;
所述路侧协同定位设备为UWB发射装置,用于发射UWB信号,仅安装于无GPS信号区域;
所述车道级地图包括港口内建筑结构信息、路侧协同设备的位置信息;
采用了GPS、激光雷达传感器、UWB等传感器,采集GPS信号、三维激光点云和UWB测距信息,结合车道级地图转换为各种不同尺度的约束,并利用因子图融合实现定位。
在高精度地图基础上,采用路侧的协同定位设备,与车载多传感器定位设备,构成面向复杂港口环境的多传感器辅助定位系统。
针对有GPS、结构丰富的环境,采用GPS和激光雷达SLAM定位的方法,如图2所示。针对无GPS、结构丰富的环境(例如地下停车场或货运仓库),采用激光雷达SLAM和UWB融合定位的方法,如图3所示。针对无GPS、结构退化的环境(例如龙门吊桥下),采用激光雷达、UWB融合定位的方法,如图4所示。
本发明的面向复杂港口环境的多传感器融合定位方法包括以下步骤:
S1,进行多传感器坐标系统一:
在隧道环境下的整个车辆定位系统中,涉及的坐标系包括世界坐标系W和LiDAR坐标系L,构建的地图基于世界坐标系
其中G是GPS定位结果集,L是LiDAR SLAM的定位结果集;
通过变换矩阵
S2,提取有GPS信号且结构丰富环境下的约束,包括以下分步骤:
S21,通过GPS接收器获取GPS信号,转换为GPS定位因子:
当车辆行驶一定距离时,GPS传感器接收一组GPS数据,在k时刻的GPS定位因子
其中
其中,
S22,利用激光雷达传感器进行SLAM定位,获取单位时间内的相对位姿因子:
基于LOAM中提出的LiDAR里程法,获得连续的LiDAR帧k和k+1之间的相对位姿测量值[Δα
其中
其中
其中
使用如下车辆运动方程,根据前两个时刻的车辆位置预测k时刻的车辆位置:
LiDAR里程计计算出的预测位置
其中,变量σ是实际情况设置。
S3,提取无GPS信号但结构丰富环境下的约束,包括以下分步骤:
S31,同步骤S22,利用激光雷达传感器进行SLAM定位,获取单位时间内的相对位姿因子:
S32,利用UWB接收器接收路侧协同定位设备发射的信号,计算UWB接收器与路侧协同定位设备之间的距离测量因子:
UWB传感器测量从路侧协同定位设备到接收器的距离,并且UWB接收器和第j个路侧协同定位设备之间的距离表示为
其中,
S4,提取无GPS信号且结构退化环境下的约束,包括以下分步骤:
S41,利用激光雷达传感器检测到墙面并计算其与墙面之间的距离,通过与地图中的墙面进行匹配,转换为距离测量因子:
在地图生成步骤中,手动测量W中所有墙面的平面方程,并由以下方程表示:
/>
其中
当激光雷达传感器扫描激光雷达点云时,通过基于RANSAC方法的平面检测方法来提取平面的激光雷达点;激光雷达传感器到第i个墙面之间的距离以
在k时刻检测到l个墙面,从第i个平面探测到的距离测量系数最大达2cm,并按
S42,同S32,利用UWB接收器接收路侧协同定位设备发射的信号,计算UWB接收器与路侧协同定位设备之间的距离测量因子。
S5,建立面向不同传感器的非线性融合定位框架,融合不同环境下通过不同传感器获取的所有非线性约束:
其中,当GPS有信号时,a
当处在结构丰富环境时,a
利用因子图原理求解上式,获取所有约束因子最小情况下的车辆状态X
在本发明的一个实施例中,集卡车辆长6.1米、宽2.4米、高2.5米,GPS、激光雷达、UWB分别安装在集卡车辆的三个方位,如图1所示。各传感器参数如下:
1.激光雷达:型号为RPLIDAR S2;扫描频率为10HZ;角度分辨率为0.12°;测距误差为±30mm。
2.GPS:型号为CodingCooper BD3U;定位精度为2.5米(CEP50,开阔地);定位更新频率为默认1HZ,最大10HZ。
3.UWB:型号为USB-S1-PRO;测距距离为600m(空旷视距);定位误差为:X轴、Y轴±10cm,Z轴±20cm;测距精度为±5cm。
- 一种面向港口环境的多源数据融合的无人驾驶汽车实时定位方法
- 一种面向港口环境的多源数据融合的无人驾驶汽车实时定位方法