Unmanned vehicle fusion positioning method based on 3D point cloud map and ESKF
-
摘要: 基于地图匹配的无人车定位方法的精度取决于已创建地图的精度，受外界的影响较小，适用于复杂场景下的无人车定位。然而目前采用的激光雷达点云匹配算法是以单一的特征为核心进行匹配，对于大规模点云匹配准确率较低，导致三维点云地图与实际环境偏差较大，造成基于地图匹配的无人车定位方法精度不高的问题。针对上述问题，提出了一种基于三维点云地图和误差状态卡尔曼滤波（ESKF）的无人车融合定位方法。该方法由三维点云地图构建和ESKF融合定位2个部分组成。在三维点云地图构建部分，通过正态分布变换（NDT）算法进行帧间点云匹配，提高大规模点云匹配准确率，并在根据激光里程计数据建立的位姿图顶点和约束边的基础上添加闭环约束构建图优化问题，采用列文伯格−马夸尔特（LM）算法进行求解，以减少位姿的累计漂移，提高三维点云地图精度。在ESKF融合定位部分，采用ESKF融合惯性测量单元（IMU）数据和三维点云地图数据，实现对无人车先验位姿（位置、姿态和速度）的修正并输出后验位姿。实验结果表明，与基于地图匹配的定位方法相比，该方法定位轨迹相对位姿误差最大值减小了0.176 9 m，平均误差减小了0.027 1 m，均方根误差减小了0.059 4 m，在定位精度和稳定性方面具有更好的表现。Abstract: The precision of the unmanned vehicle positioning method based on map matching depends on the precision of the created map. The precision is less affected by the outside world. The method is suitable for unmanned vehicle positioning in complex scenarios. However, the common laser radar point cloud matching algorithm takes single characteristic as the core for matching. The matching accuracy rate is low for large-scale point clouds. Therefore, the 3D point cloud map deviates greatly from the actual environment, resulting in the low precision of the unmanned vehicle positioning method based on map matching. To solve the above problems, an unmanned vehicle fusion positioning method based on 3D point cloud map and error state Kalman filter (ESKF) is proposed. The method is composed of two parts: 3D point cloud map construction and ESKF fusion positioning. In the 3D point cloud map construction part, inter-frame point cloud matching is performed through the normal distribution transform (NDT) algorithm to improve the accuracy of large-scale point cloud matching. The closed-loop constraint is added to construct graph optimization problem on the basis of the vertex and the constraint edge of the position and posture graph established by the laser odometer data. The Levenberg-Marquardt (LM) algorithm is used for the graph optimization solution in order to reduce the cumulative drift of the position and posture and improve the 3D point cloud map precision. In the ESKF fusion positioning part, ESKF is used to fuse inertial measurement unit (IMU) data and 3D point cloud map data. The correction of the prior position and posture (position, attitude and velocity) of the unmanned vehicle is realized and the posterior position and posture are output. The experimental results show that, compared with the method based on map matching, the maximum relative position and posture error, average error and root-mean-square error of the proposed method are reduced by 0.176 9 m, 0.027 1 m and 0.059 4 m respectively. The proposed method has better performance in positioning precision and stability.
-
0. 引言
煤矿井下环境的复杂扰动和回采过程中煤层顶板、底板的不规则变化，造成液压支架支护位姿发生变化，可能造成支护失效，导致安全事故发生[1-3]。迅速且精确地获取液压支架支护位姿信息是当前综采工作面智能化进程中亟待攻克的关键技术挑战之一[4-5]。
在液压支架姿态感知研究方面，郭周斌[6]采用压力传感器、行程传感器和倾角传感器对液压支架位姿进行感知，但未考虑到井下恶劣环境导致传感器产生误差，未采取有效的滤波手段，缺乏多传感器间的数据融合。谢明明[7]采用双轴倾角传感器对液压支架构件的倾角数据进行采集，实现了对液压支架关键构件姿态角度的计算。王忠乐[8]利用测高传感器配合倾角传感器对液压支架高度、倾角变化情况进行分析，实现了支架姿态实时监测，但仅在底座上安装1个倾角传感器，可能导致测得的姿态与真实姿态存在较大误差。谢嘉成[9]将双轴倾角传感器布置在液压支架关键构件处，实现了对液压支架的位姿监测，但在复杂工况下监测误差可能较大。张坤等[10]利用超声波传感器与九轴姿态传感器实现了对超前液压支架姿态的感知，但仅采用卡尔曼滤波算法易陷入局部极小值点，寻优结果存在误差。崔宽宽[11]将倾角传感器部署于液压支架顶梁、底座与后连杆中，通过卡尔曼滤波算法提升位姿监测的准确性，但卡尔曼滤波算法常用于求解线性问题，而液压支架位姿感知属于非线性系统状态估计问题，可能导致估计误差偏大。Chen Ningning等[12]将研制的光纤光栅倾角传感器和光纤光栅压力传感器安装于液压支架上，实现了对液压支架位姿数据和压力数据的实时监测，虽然光纤光栅传感器具有高精度和抗电磁干扰的优点，但传感器受到噪声、振动等因素的干扰，未采用滤波算法进一步提升精度。Chen Hongyue等[13]采用广角超声波传感器与倾角传感器协同监测，通过融合算法对传感器数据进行融合，实现恶劣井下环境下液压支架的姿态感知，但仅能感知得到偏航角与横滚角数据，未涉及俯仰角数据。Gao Kuidong等[14]将拉线位移传感器、视觉传感器、惯性测量单元应用于液压支架位姿感知，并引入粒子群优化算法，但视觉传感器在井下恶劣环境中易受煤炭粉尘、外界振动等因素的影响，导致位姿感知精度不高，同时所需成本较高。
为了精确感知扰动环境下液压支架位姿，本文提出一种基于多传感器融合的液压支架位姿精确感知方法。在液压支架顶梁、掩护梁、后连杆和底座4个构件上部署九轴姿态传感器采集数据，并解算出其所在构件的横滚角、俯仰角和偏航角等位姿数据；通过无迹卡尔曼滤波（Unscented Kalman Filter，UKF）算法和改进梯度下降（Improved Gradient Descent，IGD）算法对位姿数据进行滤波处理；采用自适应加权融合算法对滤波处理后的液压支架顶梁和底座的偏航角和横滚角数据进行融合处理，实现液压支架位姿精确感知。
1. 液压支架支护位姿精确感知流程
液压支架支护位姿精确感知流程如图1所示。通过在液压支架各关键构件中部署九轴姿态传感器，解算出其所在构件的横滚角、俯仰角和偏航角等位姿数据，并通过IGD−UKF算法进行滤波处理；为消除外界振动、噪声等因素引起的液压支架顶梁和底座传感器数据误差，利用自适应加权融合算法对滤波处理后的液压支架顶梁与底座横滚角和偏航角位姿数据进行融合，输出精确的液压支架位姿数据。
![]()
图 1 液压支架支护位姿精确感知流程Figure 1. Precise perception process for position and posture of hydraulic supports2. 液压支架关键构件位姿数据解算
建立地理惯性坐标系[15]，定义正东方向为xn轴正方向，正北方向为yn轴正方向，根据右手正交坐标系定则，定义垂直于大地内部指向天空为zn轴正方向；建立液压支架顶梁坐标系，定义xb轴正方向为液压支架随采煤机推移方向，zb轴正方向垂直于液压支架顶梁向上，yb轴正方向指向液压支架一侧且与xb轴、zb轴组成右手正交坐标系。将九轴姿态传感器分别布置于液压支架顶梁、掩护梁、后连杆和底座4个构件上，如图2所示。九轴姿态传感器由三轴陀螺仪、三轴加速度计、三轴磁力计组成，分别采集所安装构件的相关数据，经解算得到所安装构件的俯仰角、横滚角和偏航角等位姿数据[16-18]，并用四元数法[19]表示。
3. 基于IGD−UKF的液压支架关键构件位姿数据滤波
在液压支架支护过程中，由于液压支架工作循环中各构件间相互运动，受摩擦、振动、噪声等扰动因素影响，若仅依靠在液压支架关键构件上九轴姿态传感器的输出数据来判断液压支架位姿，会出现较大的计算误差。因此采用IGD−UKF滤波算法[20]对液压支架关键构件的位姿数据进行滤波处理。IGD−UKF算法流程如图3所示，通过状态预测和观测预测得到变量和误差协方差矩阵，并计算增益矩阵，在目标函数的迭代过程中添加扰动因子，在状态更新中利用IGD优化UKF算法的噪声协方差，输出最优后验估计，提高复杂环境下状态估计的精度。
4. 基于自适应加权融合算法的液压支架横滚角和偏航角数据融合
液压支架随采煤机向前推进过程中，因截割量存在差别会导致底板出现凹凸不平。为使液压支架保持有效支护，即液压支架顶梁与顶板贴合、底座与底板贴合，在这种情况下可能会导致液压支架顶梁与底座发生不同角度的俯仰；但对于横滚角和偏航角来说，液压支架在支护中由于自身无法发生扭转，理论上液压支架顶梁和底座的横滚角和偏航角是相同的；但在实际工作过程中，因为振动、噪声等其他外界因素的影响，液压支架顶梁和底座的横滚角和偏航角也会出现偏差。为消除偏差，采用自适应加权融合算法对权重进行自适应分配，将多个传感器数据进行融合[21]。
自适应加权融合算法通过数学优化方法在约束条件下求解最优权重，确保融合结果满足总方差最小原则。自适应加权融合算法流程如图4所示。对经IGD−UKF滤波算法处理的液压支架顶梁和底座的横滚角和偏航角数据点进行方差求解，判断是否满足总方差最小原则，若满足则根据数据点方差大小对数据点权重进行自适应分配，以保证输出最终位姿融合结果具有较高的准确性。
最终获得的融合数据可表示为
$$ x' = \sum\limits_{i = 1}^j {{\omega _i}\bar{{x_i}} \left( k \right)} $$ (1) 式中：$ \bar{{x_i}} \left( k \right) $为传感器i采集k组数据得到的均值；$ {\omega _i} $为传感器i测得数据的权值；j为传感器个数。
融合数据的方差为
$$ {\bar \sigma ^2} = \frac{1}{j}\sum\limits_{i = 1}^j {{\omega _i}\mu _i^2} $$ (2) 式中$ \mu _i^{} $为传感器i的测量值与真实值间误差。
随着权重分配的动态调整，方差趋于减小，从而提高数据精度。
5. 液压支架支护位姿精确感知实验
搭建液压支架位姿感知实验平台，如图5所示。通过顶板控制油缸带动顶板运动，以模拟顶板对液压支架顶梁施加的压力，同时顶板和底板均可前后左右倾斜，以模拟开采中顶板和底板的变化；将HWT905−CAN九轴姿态传感器分别安装在液压支架顶梁、掩护梁、后连杆和底座上，并通过RS485−USB模块建立九轴姿态传感器与数据采集平台之间的通信；通过实验控制台控制液压支架与顶板运动，液压泵站为各液压部件的伸缩提供动力；九轴姿态传感器采集液压支架在顶梁低头和抬头、底座低头和抬头、液压支架左倾和右倾、液压支架左偏和右偏等情况下的位姿数据，采集频率为10 Hz。采集数据时对液压支架顶梁进行手动敲击，以模拟振动干扰。
![]()
图 5 液压支架位姿感知实验平台Figure 5. Experimental platform for position and posture perception of hydraulic supports5.1 IGD−UKF算法滤波效果
选取九轴姿态传感器采集并解算出的底座和顶梁各600组包含俯仰角、横滚角、偏航角的数据导入Matlab软件中，分别利用UKF算法和IGD−UKF算法进行滤波处理，效果如图6所示。可看出相对于UKF算法，IGD−UKF算法处理后的数据曲线波动趋于平缓，在抑制振荡、减小振幅上的效果明显。
![]()
图 6 UKF与IGD−UKF滤波效果对比Figure 6. Comparison of filtering effects between UKF and IGD-UKF5.2 自适应加权融合算法的融合效果
将经过IGD−UKF算法处理后的液压支架顶梁与底座的横滚角、偏航角数据导入自适应加权融合算法模型中，得到的融合结果与仅经过IGD−UKF算法处理后的液压支架顶梁与底座的横滚角、偏航角进行对比，如图7所示，可看出自适应加权融合算法抑制了顶梁与底座横滚角、俯仰角数据波动。利用Matlab软件标记出图7的最小误差与最大误差，通过提取各样本点数据的绝对误差并求解平均值，得到平均绝对误差，结果见表1，可看出自适应加权融合算法提升了液压支架位姿的感知精度。
![]()
图 7 液压支架顶梁与底座姿态角融合前后效果对比Figure 7. Comparison of effects before and after fusion of attitude angles of top beam and base of hydraulic supports表 1 自适应加权融合算法融合位姿数据误差结果Table 1. Error results of position and posture data fused by adaptive weighted fusion algorithm（°） 液压支架姿态角 最小误差 最大误差 平均绝对误差 偏航角 0.001 8 0.025 1 0.004 8 横滚角 0.001 4 0.028 1 0.004 7 6. 结论
1） 将九轴姿态传感器安装于液压支架顶梁、掩护梁、后连杆和底座进行数据采集并解算出所在构件的位姿数据，利用IGD−UKF算法对位姿数据进行滤波处理，抑制了数据振荡。通过自适应加权融合算法对滤波处理后不同构件的相同位姿数据进行融合，以减小振动等对液压支架位姿感知精度的干扰。
2） 搭建了液压支架位姿感知实验平台，开展了IGD−UKF算法滤波实验和自适应加权融合算法融合实验，得出液压支架偏航角误差为0.001 8～0.025 1°，平均绝对误差为0.004 8°，横滚角误差为0.001 4～0.028 1°，平均绝对误差为0.004 7°，实现了液压支架位姿的精确感知。
-
![]()
图 5 单一特征建图算法构建的三维点云地图
Figure 5. 3D point cloud map constructed by single characteristic mapping algorithm
![]()
图 7 基于 KITTI 数据集的融合定位轨迹
Figure 7. Fusion positioning trajectory based on KITTI dataset
![]()
图 8 不同定位方法的相对位姿误差对比
Figure 8. Comparison of relative pose error of different positioning methods
![]()
图 9 基于校园实测数据的融合定位轨迹
Figure 9. Fusion positioning trajectory based on campus measured data
![]()
图 10 不同定位方法的定位轨迹对比
Figure 10. Comparison of positioning trajectories of different positioning methods
表 1 定位轨迹误差统计结果
Table 1 Positioning trajectory error statistics
m 方法 相对位姿误差
最大值平均
误差均方根
误差基于地图匹配的定位 1.532 6 0.680 3 0.780 9 ESKF融合定位 1.355 7 0.653 2 0.721 5 
下载: 导出CSV
-
[1] LIU Ran,YUEN C,DO T N,et al. Fusing similarity-based sequence and dead reckoning for indoor positioning without training[J]. IEEE Sensors Journal,2017,17(13):4197-4207. DOI: 10.1109/JSEN.2017.2706303
[2] 杨磊,庞硕,杨耀民,等. 基于9DOF IMU的AUV惯性导航技术研究[J]. 计算机测量与控制,2016,24(3):133-135,153. YANG Lei,PANG Shuo,YANG Yaomin,et al. Study of navigation for AUV based on 9DOF IMU[J]. Computer Measurement & Control,2016,24(3):133-135,153.
[3] 方辉. GPS信号遮挡区的智能车定位方法研究[D]. 上海: 上海交通大学, 2010. FANG Hui. Research on intelligent vehicle localization in GPS signal blocked areas[D]. Shanghai: Shanghai Jiao Tong University, 2010.
[4] 顾文华,周波,戴先中. 基于ICP匹配算法的室内移动机器人定位[J]. 华中科技大学学报(自然科学版),2013,41(增刊1):262-266. GU Wenhua,ZHOU Bo,DAI Xianzhong. ICP matching algorithm-based localization of indoor mobile robots[J]. Journal of Huazhong University of Science and Technology(Natural Science Edition),2013,41(S1):262-266.
[5] 杨林,马宏伟,王岩,等. 煤矿巡检机器人同步定位与地图构建方法研究[J]. 工矿自动化,2019,45(9):18-24. DOI: 10.13272/j.issn.1671-251x.17444 YANG Lin,MA Hongwei,WANG Yan,et al. Research on method of simultaneous localization and mapping of coal mine inspection robot[J]. Industry and Mine Automation,2019,45(9):18-24. DOI: 10.13272/j.issn.1671-251x.17444
[6] 周风余,庄文密,万方,等. 基于路标与云架构的多机器人建图及融合方法[J]. 华中科技大学学报(自然科学版),2020,48(11):30-36. ZHOU Fengyu,ZHUANG Wenmi,WAN Fang,et al. Multi-robot mapping and merging method based on landmarks and cloud architecture[J]. Journal of Huazhong University of Science and Technology(Natural Science Edition),2020,48(11):30-36.
[7] HENTSCHEL M, WAGNER B. Autonomous robot navigation based on OpenStreetMap geodata[C]. The 13th International IEEE Conference on Intelligent Transportation Systems, Funchal, 2010: 1645-1650.
[8] MOOSMANN F, STILLER C. Velodyne SLAM[C]. IEEE Intelligent Vehicles Symposium, Baden-Baden, 2011: 393-398.
[9] YANG Jiaolong, LI Hongdong, JIA Yunde. Go-ICP: solving 3D registration efficiently and globally optimally[C]. IEEE International Conference on Computer Vision, Sydney, 2013: 1457-1464.
[10] DROESCHEL D,SCHWARZ M,BEHNKE S. Continuous mapping and localization for autonomous navigation in rough terrain using a 3D laser scanner[J]. Robotics and Autonomous Systems,2017,88:104-115. DOI: 10.1016/j.robot.2016.10.017
[11] BLOESCH M,BURRI M,OMARI S,et al. Iterated extended Kalman filter based visual-inertial odometry using direct photometric feedback[J]. International Journal of Robotics Research,2017,36(10):1053-1072. DOI: 10.1177/0278364917728574
[12] 冯敬伟,车明明,马巍,等. 基于激光测距仪与微惯性测量单元的室内个人导航方法[J]. 计算机工程,2016,42(8):311-315. DOI: 10.3969/j.issn.1000-3428.2016.08.055 FENG Jingwei,CHE Mingming,MA Wei,et al. Indoor personal navigation method based on laser range finder and micro-inertial measurement unit[J]. Computer Engineering,2016,42(8):311-315. DOI: 10.3969/j.issn.1000-3428.2016.08.055
[13] 余洪山,付强,孙健,等. 面向室内移动机器人的改进3D−NDT点云配准算法[J]. 仪器仪表学报,2019,40(9):151-161. DOI: 10.19650/j.cnki.cjsi.J1905346 YU Hongshan,FU Qiang,SUN Jian,et al. Improved 3D-NDT point cloud registration algorithm for indoor mobile robot[J]. Chinese Journal of Scientific Instrument,2019,40(9):151-161. DOI: 10.19650/j.cnki.cjsi.J1905346
[14] 吴环宇. 基于激光雷达的智能车辆三维地图构建和定位算法研究[D]. 长春: 吉林大学, 2020. WU Huanyu. Research on 3D mapping and localization algorithm of intelligent vehicle based on lidar[D]. Changchun: Jilin University, 2020.
[15] WAN Guowei, YANG Xiaolong, CAI Renlan, et al. Robust and precise vehicle localization based on multi-sensor fusion in diverse city scenes[C]. IEEE International Conference on Robotics and Automation, Brisbane, 2018: 4670-4677.
计量
- 文章访问数: 266
- HTML全文浏览量: 53
- PDF下载量: 41
