CN115951369A - 面向复杂港口环境的多传感器融合定位方法 - Google Patents

面向复杂港口环境的多传感器融合定位方法 Download PDF

Info

Publication number
CN115951369A
CN115951369A CN202211604925.8A CN202211604925A CN115951369A CN 115951369 A CN115951369 A CN 115951369A CN 202211604925 A CN202211604925 A CN 202211604925A CN 115951369 A CN115951369 A CN 115951369A
Authority
CN
China
Prior art keywords
gps
positioning
coordinate system
environment
vehicle
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
Application number
CN202211604925.8A
Other languages
English (en)
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.)
Changjia Fengxing Suzhou Intelligent Technology Co ltd
Original Assignee
Changjia Fengxing Suzhou Intelligent Technology 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 Changjia Fengxing Suzhou Intelligent Technology Co ltd filed Critical Changjia Fengxing Suzhou Intelligent Technology Co ltd
Priority to CN202211604925.8A priority Critical patent/CN115951369A/zh
Publication of CN115951369A publication Critical patent/CN115951369A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • 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

  • Position Fixing By Use Of Radio Waves (AREA)
  • Optical Radar Systems And Details Thereof (AREA)

Abstract

本发明公开了一种面向复杂港口环境的多传感器融合定位方法，包括：S1，进行多传感器坐标***一；S2，提取有GPS信号且结构丰富环境下的约束；S3，提取无GPS信号但结构丰富环境下的约束；S4，提取无GPS信号且结构退化环境下的约束；S5，建立面向不同传感器的非线性融合定位框架，融合不同环境下通过不同传感器获取的所有非线性约束。该方法实现了在场景特征变换、环境变换情况下车辆无缝链接定位的问题，有效减少了线性化的误差，解决了结构退化环境下的定位问题。

Description

面向复杂港口环境的多传感器融合定位方法
技术领域
本发明涉及定位技术领域，具体涉及一种面向复杂港口环境的多传感器融合定位方法。
背景技术
由于自主导航技术，特别是在GPS盲区环境下的机器人定位技术的进步，使机器人在检测任务中的应用迅速发展。虽然已经提出了许多方法来使用摄像机和激光雷达等机载传感器对机器人进行定位，但在几何退化的环境中，如隧道中，实现鲁棒定位仍然是一个具有挑战性的问题。
激光雷达获得的数据具有精度高，实时好，数据稳定等特点，且激光雷达安装方便，适合作为小型环境感知***的探测器。LOAM(Lidar Odometry and Mapping)算法，是一种基于激光雷达的SLAM算法。然而，由于激光雷达通过扫描环境来捕获几何信息，因此在隧道等几何退化的情况下，它更有可能受到影响。
UWB是近年来新兴的一种无线通信技术。基于UWB的测距的关键优势是，宽带能够以比使用其他无线技术更高的精度测量ToA和RTT。近年来，基于UWB的空间定位技术在多个军民领域中实现了广泛应用。UWB能有效地解决室内GPS信号遮挡的问题，但不适用于全局定位。
由于场景的复杂性，单一传感器无法解决所有场景下的定位问题，往往需要多种传感器进行融合定位。激光传感器数据精度高，环境适应性强，但是无法分辨几何上连续重复的场景，与UWB传感器数据融合能够在退化环境进行定位。
传统的融合框架，如卡尔曼滤波器(包括其变体扩展卡尔曼滤波器(EKF)、无迹卡尔曼滤波器(UKF)、不变扩展卡尔曼滤波算法、不变EKF)，由于技术成熟、实现简单、计算效率高而非常受欢迎。然而，在隧道中，通过EKF集成GNSS/INS的性能显著下降。
此外，现有的融合框架较为单一，仅能解决单一场景下的定位问题，如针对退化环境的定位与针对无GPS信号场景下的定位，但缺乏一个通用的融合框架，以实现在不同环境下的无缝衔接的高精度车辆定位。
发明内容
为了克服上述现有技术的不足，本发明提供一种面向复杂港口环境的多传感器融合定位方法。
为此，本发明采用以下技术方案：
一种面向复杂港口环境的多传感器融合定位方法，包括以下步骤：
S1，进行多传感器坐标***一：构建的地图基于世界坐标系
Figure BDA0003998256850000021
车辆坐标系基于LiDAR坐标系，通过变换矩阵
Figure BDA0003998256850000022
将LiDAR里程仪的定位结果转换到世界坐标系
Figure BDA0003998256850000023
中，并通过SVD分解方法将LiDAR坐标系与GPS坐标系之间的变换矩阵
Figure BDA0003998256850000024
分解为旋转矩阵
Figure BDA0003998256850000025
和平移矩阵
Figure BDA0003998256850000026
S2，提取有GPS信号且结构丰富环境下的约束，包括以下分步骤：
S21，通过GPS接收器获取GPS信号，转换为GPS定位因子：
当车辆行驶一定距离时，GPS传感器接收一组GPS数据，在k时刻的GPS定位因子
Figure BDA0003998256850000027
定义如下：
Figure BDA0003998256850000028
Figure BDA0003998256850000029
其中
Figure BDA00039982568500000210
是k时刻的GPS定位结果，包括车辆的x、y和z坐标；
Figure BDA00039982568500000211
是k时刻车辆状态中的平移向量，也是车辆位置，为未知量，包括车辆的x、y和z坐标；用协方差矩阵
Figure BDA00039982568500000212
对定位因子进行加权，
Figure BDA00039982568500000213
设计如下：
Figure BDA00039982568500000214
其中，
Figure BDA00039982568500000215
和
Figure BDA00039982568500000216
是GPS定位在x、y和z方向上的权重，均为标量，由x、y和z方向上的GPS定位精度和GPS定位状态确定；
S22，利用激光雷达传感器进行SLAM定位，获取单位时间内的相对位姿因子：
基于LOAM中提出的LiDAR里程法，获得连续的LiDAR帧k和k+1之间的相对位姿测量值[ΔαkΔβk]T，其中
Figure BDA00039982568500000217
时刻的相对位姿因子
Figure BDA00039982568500000218
定义为：
Figure BDA00039982568500000219
Figure BDA00039982568500000220
其中
Figure BDA0003998256850000031
是k-1、k时刻车辆状态中的平移向量；
Figure BDA0003998256850000032
是k-1、k时刻车辆状态中的旋转向量，均为未知量；
Figure BDA0003998256850000033
是世界坐标系
Figure BDA0003998256850000034
和激光雷达坐标系
Figure BDA0003998256850000035
之间的旋转矩阵，由S1中计算的旋转矩阵
Figure BDA0003998256850000036
代替；用协方差矩阵
Figure BDA0003998256850000037
对相对姿态因子进行加权；
S3，提取无GPS信号但结构丰富环境下的约束；
S4，提取无GPS信号且结构退化环境下的约束；
S5，建立面向不同传感器的非线性融合定位框架，融合不同环境下通过不同传感器获取的所有非线性约束：
Figure BDA0003998256850000038
其中，当GPS有信号时，a1＝1,a3＝0，否则，a1＝0,a3＝1；
当处在结构丰富环境时，a0＝1,a2＝0，否则，a0＝0,a2＝1；
利用因子图原理求解上式，获取所有约束因子最小情况下的车辆状态Xk＝[tk rk]T,
Figure BDA0003998256850000039
上述步骤S1中所述变换矩阵
Figure BDA00039982568500000310
表示如下：
Figure BDA00039982568500000311
其中G是GPS定位结果集，L是LiDAR SLAM的定位结果集。
上述的步骤S22中，步骤S22中，协方差矩阵
Figure BDA00039982568500000312
按以下方式设计：
Figure BDA00039982568500000313
其中
Figure BDA00039982568500000314
是旋转的权重，通过平面检测进行校正；
Figure BDA00039982568500000315
是转换的权重，按如下方式设计：
Figure BDA00039982568500000316
其中
Figure BDA00039982568500000317
为标量，其计算方法如下：
使用如下车辆运动方程，根据前两个时刻的车辆位置预测k时刻的车辆位置：
Figure BDA00039982568500000318
LiDAR里程计计算出的预测位置
Figure BDA00039982568500000319
与位置tk之间的误差距离与权重
Figure BDA00039982568500000320
有关，误差距离越大，权重越小；使用高斯模型根据误差距离对权重
Figure BDA0003998256850000041
进行建模，如下所示：
Figure BDA0003998256850000042
其中，变量σ是实际情况设置。
上述的步骤S3包括以下分步骤：
S31，同步骤S22，利用激光雷达传感器进行SLAM定位，获取单位时间内的相对位姿因子：
S32，利用UWB接收器接收路侧协同定位设备发射的信号，计算UWB接收器与路侧协同定位设备之间的距离测量因子：
UWB传感器测量从路侧协同定位设备到接收器的距离，并且UWB接收器和第j个路侧协同定位设备之间的距离表示为
Figure BDA0003998256850000043
为标量；假设接收器位于LiDAR坐标系L的原点，则来自UWB接收器的距离测量因子由如下公式定位：
Figure BDA0003998256850000044
Figure BDA0003998256850000045
其中，
Figure BDA0003998256850000046
是第j个路侧协同定位设备Uj在
Figure BDA0003998256850000047
坐标系的位置，来自路侧协同定位设备的距离测量系数最大为5厘米，由
Figure BDA0003998256850000048
进行加权，
Figure BDA0003998256850000049
为标量。因此，
Figure BDA00039982568500000410
应该有更高的权重。
上述的步骤S4包括以下分步骤：
S41，利用激光雷达传感器检测到墙面并计算其与墙面之间的距离，通过与地图中的墙面进行匹配，转换为距离测量因子：
在地图生成步骤中，手动测量W中所有墙面的平面方程，并由以下方程表示：
Figure BDA00039982568500000411
其中
Figure BDA00039982568500000412
是第i个平面的法向量，
Figure BDA00039982568500000413
和
Figure BDA00039982568500000414
为法向量在x，y和z方向上的分量，均为标量；di是第i个平面的截距，为标量；
当激光雷达传感器扫描激光雷达点云时，通过基于RANSAC方法的平面检测方法来提取平面的激光雷达点；激光雷达传感器到第i个墙面之间的距离以
Figure BDA00039982568500000415
表示，
Figure BDA00039982568500000416
为标量；来自平面检测的距离测量系数由如下公式定义：
Figure BDA0003998256850000051
Figure BDA0003998256850000052
在k时刻检测到l个墙面，从第i个平面探测到的距离测量系数最大达2cm，并按
Figure BDA0003998256850000053
加权，
Figure BDA0003998256850000054
为标量。因此，
Figure BDA0003998256850000055
应该有更高的权重。
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，构建的地图基于世界坐标系
Figure BDA0003998256850000061
车辆坐标系基于LiDAR坐标系，通过变换矩阵
Figure BDA0003998256850000062
确定GPS接收器与UWB接收器在LiDAR坐标系中的位置，激光雷达传感器接收一组激光雷达数据，以第一帧的激光雷达坐标系为LiDAR坐标系，LiDAR坐标系与GPS坐标系之间的变换矩阵
Figure BDA0003998256850000063
由如下表示：
Figure BDA0003998256850000064
其中G是GPS定位结果集，L是LiDAR SLAM的定位结果集；
通过变换矩阵
Figure BDA0003998256850000065
将LiDAR里程仪的定位结果转换到世界坐标系
Figure BDA0003998256850000066
中，并通过SVD分解方法将LiDAR坐标系与GPS坐标系之间的变换矩阵
Figure BDA0003998256850000067
分解为旋转矩阵
Figure BDA0003998256850000071
和平移矩阵
Figure BDA0003998256850000072
S2，提取有GPS信号且结构丰富环境下的约束，包括以下分步骤：
S21，通过GPS接收器获取GPS信号，转换为GPS定位因子：
当车辆行驶一定距离时，GPS传感器接收一组GPS数据，在k时刻的GPS定位因子
Figure BDA0003998256850000073
定义如下：
Figure BDA0003998256850000074
Figure BDA0003998256850000075
其中
Figure BDA0003998256850000076
是k时刻的GPS定位结果，包括车辆的x、y和z坐标；
Figure BDA0003998256850000077
是k时刻车辆状态中的平移向量，也是车辆位置，为未知量，包括车辆的x、y和z坐标；用协方差矩阵
Figure BDA0003998256850000078
对定位因子进行加权，
Figure BDA0003998256850000079
设计如下：
Figure BDA00039982568500000710
其中，
Figure BDA00039982568500000711
和
Figure BDA00039982568500000712
是GPS定位在x、y和z方向上的权重，均为标量，由x、y和z方向上的GPS定位精度和GPS定位状态确定；
S22，利用激光雷达传感器进行SLAM定位，获取单位时间内的相对位姿因子：
基于LOAM中提出的LiDAR里程法，获得连续的LiDAR帧k和k+1之间的相对位姿测量值[ΔαkΔβk]T，其中
Figure BDA00039982568500000713
时刻的相对位姿因子
Figure BDA00039982568500000714
定义为：
Figure BDA00039982568500000715
Figure BDA00039982568500000716
其中
Figure BDA00039982568500000717
是k-1、k时刻车辆状态中的平移向量；
Figure BDA00039982568500000718
是k-1、k时刻车辆状态中的旋转向量，均为未知量；
Figure BDA00039982568500000719
是世界坐标系
Figure BDA00039982568500000720
和激光雷达坐标系
Figure BDA00039982568500000721
之间的旋转矩阵，由S1中计算的旋转矩阵
Figure BDA00039982568500000722
代替；用协方差矩阵
Figure BDA00039982568500000723
对相对姿态因子进行加权。协方差矩阵
Figure BDA00039982568500000724
按以下方式设计：
Figure BDA00039982568500000725
其中
Figure BDA0003998256850000081
是旋转的权重，通过平面检测进行校正；
Figure BDA0003998256850000082
是转换的权重，按如下方式设计：
Figure BDA0003998256850000083
其中
Figure BDA0003998256850000084
为标量，其计算方法如下：
使用如下车辆运动方程，根据前两个时刻的车辆位置预测k时刻的车辆位置：
Figure BDA0003998256850000085
LiDAR里程计计算出的预测位置
Figure BDA0003998256850000086
与位置tk之间的误差距离与权重
Figure BDA0003998256850000087
有关，误差距离越大，权重越小；使用高斯模型根据误差距离对权重
Figure BDA0003998256850000088
进行建模，如下所示：
Figure BDA0003998256850000089
其中，变量σ是实际情况设置。
S3，提取无GPS信号但结构丰富环境下的约束，包括以下分步骤：
S31，同步骤S22，利用激光雷达传感器进行SLAM定位，获取单位时间内的相对位姿因子：
S32，利用UWB接收器接收路侧协同定位设备发射的信号，计算UWB接收器与路侧协同定位设备之间的距离测量因子：
UWB传感器测量从路侧协同定位设备到接收器的距离，并且UWB接收器和第j个路侧协同定位设备之间的距离表示为
Figure BDA00039982568500000810
为标量；假设接收器位于LiDAR坐标系L的原点，则来自UWB接收器的距离测量因子由如下公式定位：
Figure BDA00039982568500000811
Figure BDA00039982568500000812
其中，
Figure BDA00039982568500000813
是第j个路侧协同定位设备Uj在
Figure BDA00039982568500000814
坐标系的位置，来自路侧协同定位设备的距离测量系数最大为5厘米，由
Figure BDA00039982568500000815
进行加权，
Figure BDA00039982568500000816
为标量。
S4，提取无GPS信号且结构退化环境下的约束，包括以下分步骤：
S41，利用激光雷达传感器检测到墙面并计算其与墙面之间的距离，通过与地图中的墙面进行匹配，转换为距离测量因子：
在地图生成步骤中，手动测量W中所有墙面的平面方程，并由以下方程表示：
Figure BDA0003998256850000091
其中
Figure BDA0003998256850000092
是第i个平面的法向量，
Figure BDA0003998256850000093
和
Figure BDA0003998256850000094
为法向量在x，y和z方向上的分量，均为标量；di是第i个平面的截距，为标量；
当激光雷达传感器扫描激光雷达点云时，通过基于RANSAC方法的平面检测方法来提取平面的激光雷达点；激光雷达传感器到第i个墙面之间的距离以
Figure BDA0003998256850000095
表示，
Figure BDA0003998256850000096
为标量；来自平面检测的距离测量系数由如下公式定义：
Figure BDA0003998256850000097
Figure BDA0003998256850000098
在k时刻检测到l个墙面，从第i个平面探测到的距离测量系数最大达2cm，并按
Figure BDA0003998256850000099
加权，
Figure BDA00039982568500000910
为标量；
S42，同S32，利用UWB接收器接收路侧协同定位设备发射的信号，计算UWB接收器与路侧协同定位设备之间的距离测量因子。
S5，建立面向不同传感器的非线性融合定位框架，融合不同环境下通过不同传感器获取的所有非线性约束：
Figure BDA00039982568500000911
其中，当GPS有信号时，a1＝1,a3＝0，否则，a1＝0,a3＝1；
当处在结构丰富环境时，a0＝1,a2＝0，否则，a0＝0,a2＝1；
利用因子图原理求解上式，获取所有约束因子最小情况下的车辆状态Xk＝[tk rk]T,
Figure BDA00039982568500000912
在本发明的一个实施例中，集卡车辆长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。

Claims (5)

1.一种面向复杂港口环境的多传感器融合定位方法，包括以下步骤：
S1，进行多传感器坐标***一：构建的地图基于世界坐标系
Figure FDA0003998256840000011
车辆坐标系基于LiDAR坐标系，通过变换矩阵
Figure FDA0003998256840000012
将LiDAR里程仪的定位结果转换到世界坐标系
Figure FDA0003998256840000013
中，并通过SVD分解方法将LiDAR坐标系与GPS坐标系之间的变换矩阵
Figure FDA0003998256840000014
分解为旋转矩阵
Figure FDA0003998256840000015
和平移矩阵
Figure FDA0003998256840000016
S2，提取有GPS信号且结构丰富环境下的约束，包括以下分步骤：
S21，通过GPS接收器获取GPS信号，转换为GPS定位因子：
当车辆行驶一定距离时，GPS传感器接收一组GPS数据，在k时刻的GPS定位因子
Figure FDA0003998256840000017
定义如下：
Figure FDA0003998256840000018
Figure FDA0003998256840000019
其中
Figure FDA00039982568400000110
是k时刻的GPS定位结果，包括车辆的x、y和z坐标；
Figure FDA00039982568400000111
是k时刻车辆状态中的平移向量，也是车辆位置，为未知量，包括车辆的x、y和z坐标；用协方差矩阵
Figure FDA00039982568400000112
对定位因子进行加权，
Figure FDA00039982568400000113
设计如下：
Figure FDA00039982568400000114
其中，
Figure FDA00039982568400000115
和
Figure FDA00039982568400000116
是GPS定位在x、y和z方向上的权重，均为标量，由x、y和z方向上的GPS定位精度和GPS定位状态确定；
S22，利用激光雷达传感器进行SLAM定位，获取单位时间内的相对位姿因子：
基于LOAM中提出的LiDAR里程法，获得连续的LiDAR帧k和k+1之间的相对位姿测量值[ΔαkΔβk]T，其中
Figure FDA00039982568400000117
时刻的相对位姿因子
Figure FDA00039982568400000118
定义为：
Figure FDA00039982568400000119
Figure FDA00039982568400000120
其中
Figure FDA00039982568400000121
是k-1、k时刻车辆状态中的平移向量；
Figure FDA00039982568400000122
是k-1、k时刻车辆状态中的旋转向量，均为未知量；
Figure FDA0003998256840000021
是世界坐标系
Figure FDA0003998256840000022
和激光雷达坐标系
Figure FDA0003998256840000023
之间的旋转矩阵，由S1中计算的旋转矩阵
Figure FDA0003998256840000024
代替；用协方差矩阵
Figure FDA0003998256840000025
对相对姿态因子进行加权；
S3，提取无GPS信号但结构丰富环境下的约束；
S4，提取无GPS信号且结构退化环境下的约束；
S5，建立面向不同传感器的非线性融合定位框架，融合不同环境下通过不同传感器获取的所有非线性约束：
Figure FDA0003998256840000026
其中，当GPS有信号时，a1＝1,a3＝0，否则，a1＝0,a3＝1；
当处在结构丰富环境时，a0＝1,a2＝0，否则，a0＝0,a2＝1；
利用因子图原理求解上式，获取所有约束因子最小情况下的车辆状态
Figure FDA0003998256840000027
2.根据权利要求1所述的多传感器融合定位方法，其特征在于：步骤S1中所述变换矩阵
Figure FDA0003998256840000028
表示如下：
Figure FDA0003998256840000029
其中G是GPS定位结果集，L是LiDAR SLAM的定位结果集。
3.根据权利要求1所述的多传感器融合定位方法，其特征在于：步骤S22中，协方差矩阵
Figure FDA00039982568400000210
按以下方式设计：
Figure FDA00039982568400000211
其中
Figure FDA00039982568400000212
是旋转的权重，通过平面检测进行校正；
Figure FDA00039982568400000213
是转换的权重，按如下方式设计：
Figure FDA00039982568400000214
其中
Figure FDA00039982568400000215
为标量，其计算方法如下：
使用如下车辆运动方程，根据前两个时刻的车辆位置预测k时刻的车辆位置：
Figure FDA00039982568400000216
LiDAR里程计计算出的预测位置
Figure FDA00039982568400000217
与位置tk之间的误差距离与权重
Figure FDA00039982568400000218
有关，误差距离越大，权重越小；使用高斯模型根据误差距离对权重
Figure FDA0003998256840000031
进行建模，如下所示：
Figure FDA0003998256840000032
其中，变量σ是实际情况设置。
4.根据权利要求1所述的多传感器融合定位方法，其特征在于，步骤S3包括以下分步骤：
S31，同步骤S22，利用激光雷达传感器进行SLAM定位，获取单位时间内的相对位姿因子：
S32，利用UWB接收器接收路侧协同定位设备发射的信号，计算UWB接收器与路侧协同定位设备之间的距离测量因子：
UWB传感器测量从路侧协同定位设备到接收器的距离，并且UWB接收器和第j个路侧协同定位设备之间的距离表示为
Figure FDA0003998256840000033
Figure FDA0003998256840000034
为标量；假设接收器位于LiDAR坐标系L的原点，则来自UWB接收器的距离测量因子由如下公式定位：
Figure FDA0003998256840000035
Figure FDA0003998256840000036
其中，
Figure FDA0003998256840000037
是第j个路侧协同定位设备Uj在
Figure FDA0003998256840000038
坐标系的位置，来自路侧协同定位设备的距离测量系数最大为5厘米，由
Figure FDA0003998256840000039
进行加权，
Figure FDA00039982568400000310
为标量。
5.根据权利要求1所述的多传感器融合定位方法，其特征在于，步骤S4包括以下分步骤：
S41，利用激光雷达传感器检测到墙面并计算其与墙面之间的距离，通过与地图中的墙面进行匹配，转换为距离测量因子：
在地图生成步骤中，手动测量W中所有墙面的平面方程，并由以下方程表示：
Figure FDA00039982568400000311
其中
Figure FDA00039982568400000312
是第i个平面的法向量，
Figure FDA00039982568400000313
和
Figure FDA00039982568400000314
为法向量在x，y和z方向上的分量，均为标量；di是第i个平面的截距，为标量；
当激光雷达传感器扫描激光雷达点云时，通过基于RANSAC方法的平面检测方法来提取平面的激光雷达点；激光雷达传感器到第i个墙面之间的距离以
Figure FDA0003998256840000041
表示，
Figure FDA0003998256840000042
为标量；来自平面检测的距离测量系数由如下公式定义：
Figure FDA0003998256840000043
Figure FDA0003998256840000044
在k时刻检测到l个墙面，从第i个平面探测到的距离测量系数最大达2cm，并按
Figure FDA0003998256840000045
加权，
Figure FDA0003998256840000046
为标量；
S42，同S32，利用UWB接收器接收路侧协同定位设备发射的信号，计算UWB接收器与路侧协同定位设备之间的距离测量因子。
CN202211604925.8A 2022-12-14 2022-12-14 面向复杂港口环境的多传感器融合定位方法 Pending CN115951369A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202211604925.8A CN115951369A (zh) 2022-12-14 2022-12-14 面向复杂港口环境的多传感器融合定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202211604925.8A CN115951369A (zh) 2022-12-14 2022-12-14 面向复杂港口环境的多传感器融合定位方法

Publications (1)

Publication Number Publication Date
CN115951369A true CN115951369A (zh) 2023-04-11

Family

ID=87290028

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202211604925.8A Pending CN115951369A (zh) 2022-12-14 2022-12-14 面向复杂港口环境的多传感器融合定位方法

Country Status (1)

Country Link
CN (1) CN115951369A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117268409A (zh) * 2023-08-30 2023-12-22 苏州大成运和智能科技有限公司 基于迭代扩展卡尔曼滤波的无人车局部定位方法

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117268409A (zh) * 2023-08-30 2023-12-22 苏州大成运和智能科技有限公司 基于迭代扩展卡尔曼滤波的无人车局部定位方法

Similar Documents

Publication Publication Date Title
CN108225302B (zh) 一种石化工厂巡检机器人定位***和方法
CN110243358B (zh) 多源融合的无人车室内外定位方法及***
CN105628026B (zh) 一种移动物体的定位定姿方法和***
CN109911188B (zh) 非卫星导航定位环境的桥梁检测无人机***
CN110837080B (zh) 激光雷达移动测量***的快速标定方法
CN113124856B (zh) 基于uwb在线锚点的视觉惯性紧耦合里程计及计量方法
CN110702091B (zh) 一种沿地铁轨道移动机器人的高精度定位方法
CN114199240B (zh) 无gps信号下二维码、激光雷达与imu融合定位***及方法
CN112987065B (zh) 一种融合多传感器的手持式slam装置及其控制方法
CN108775901B (zh) 一种实时slam场景地图构建***、导航***及方法
CN109974694B (zh) 一种基于uwb/imu/气压计的室内行人3d定位方法
CN112967392A (zh) 一种基于多传感器触合的大规模园区建图定位方法
CN111077907A (zh) 一种室外无人机的自主定位方法
CN114459467B (zh) 一种未知救援环境中基于vi-slam的目标定位方法
CN114674311B (zh) 一种室内定位与建图方法及***
CN111694001A (zh) 一种用于无人机的实时测距定位***
CN115950418A (zh) 多传感器融合定位方法
CN115951369A (zh) 面向复杂港口环境的多传感器融合定位方法
CN116242372B (zh) 一种gnss拒止环境下的uwb-激光雷达-惯导融合定位方法
Kong et al. Hybrid indoor positioning method of BLE and monocular VINS based smartphone
CN116105726A (zh) 一种多传感器融合的爬壁机器人立面定位方法
CN115930948A (zh) 一种果园机器人融合定位方法
CN114812554A (zh) 基于滤波的多源融合机器人室内绝对定位方法
CN112798020A (zh) 一种用于评估智能汽车定位精度的***及方法
Yao et al. Tightly coupled indoor positioning using UWB/mmWave radar/IMU

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