CN117234203A - 一种多源里程融合slam井下导航方法 - Google Patents

Abstract

本发明公开了一种多源里程融合SLAM井下导航方法，根据井下无人车的外形结构，布置导航装置；对导航装置进行联合参数标定；井下无人车根据多类型数据采集装置进行数据采集，获取井下无人车多类型里程计信息和多类型井下巷道环境感知信息，将多源里程计信息融合获取车辆高精度姿态信息，井下无人车根据自身高精度姿态信息和多类型井下巷道环境感知数据进行融合定位，并构建SLAM高精度三维地图；井下无人车根据给定目标点、自身姿态信息和SLAM三维地图进行路径规划，获取从当前位置到目标点的目标路径信息；井下无人车根据自身姿态信息、位置信息和目标路径信息生成导航控制指令，以控制其底盘按照目标路径进行移动。

Description

一种多源里程融合SLAM井下导航方法

技术领域

本发明涉及一种多源里程融合SLAM井下导航方法，属于导航定位技术领域。

背景技术

井下SLAM技术是指在井下或类似复杂环境中进行同时定位与地图构建的技术，其是井下无人车实现自主导航的重要技术构成。在这种环境中，传统的导航方法如全球定位系统(GPS)往往无法准确工作，因为GPS信号很容易受到井下结构和地质条件的干扰而失去精度。因此，井下SLAM技术成为一种重要的解决方案。传统的SLAM技术基于单一传感器，如激光雷达或摄像头，来同时定位机器人/车辆并构建环境地图。在井下环境中，可能存在着多种挑战，例如光照不足、浊度高的水、尘埃和烟尘、复杂的障碍物等。这些因素会影响传感器的性能和数据质量，限制了传统SLAM方法的可靠性和精度。因此，井下环境由于局限性和复杂性，导致了单一SLAM方法的一些困难和局限性。为了解决这些问题，提出了多源里程融合SLAM导航方法，实现井下无人车更好的定位和地图构建精度，提高井下无人车自主导航、路径规划和控制的机动性与灵活性。

发明内容

针对上述现有技术存在的问题，本发明提供一种多源里程融合SLAM井下导航方法，为井下无人车提供更准确的定位结果，可以动态地选择和调整传感器权重，以适应不同的场景和条件，机动性和灵活性好。

为了实现上述目的，本发明采用的技术方案是：一种多源里程融合SLAM井下导航方法，包括以下步骤：

S201：根据井下无人车的外形结构，布置导航装置；

S202：对导航装置进行联合参数标定；

S203：井下无人车根据多类型数据采集装置进行数据采集，获取井下无人车多类型里程计信息和多类型井下巷道环境感知信息，将多源里程计信息融合获取车辆高精度姿态信息，井下无人车根据自身高精度姿态信息和多类型井下巷道环境感知数据进行融合定位，并构建SLAM高精度三维地图；

S204：井下无人车根据给定目标点、自身姿态信息和SLAM三维地图进行路径规划，获取从当前位置到目标点的目标路径信息，且对规划路径进行优化；

S205：井下无人车根据自身姿态信息、位置信息和目标路径信息生成导航控制指令，以控制其底盘按照目标路径进行移动。

进一步的，所述步骤S201中的导航装置包括上层控制单元、底层控制单元和无人车底盘；

上层控制单元包括IMU、深度相机传感器、激光雷达、远程PC、无线网卡和主控制器，IMU、深度相机传感器、激光雷达与主控制器连接，无线网卡设置在主控制器中，远程PC通过无线路由器和主控制器连接；

底层控制单元包括车载控制器、转向电机控制器和驱动电机控制器，转向电机控制器、驱动电机控制器与车载控制器通过can接口连接，主控制器与车载控制器通过can接口连接；

无人车底盘包括轮式底盘、霍尔编码器、减速电机和悬架，减速电机内置于井下无人车车轮中，霍尔编码器与减速电机连接，左右两个悬架的上端和下端均与轮式底盘连接。

进一步的，所述步骤S203中多类型里程计分别为激光里程计、视觉里程计、轮式里程计和IMU里程计；井下无人车根据自身高精度姿态信息和多类型井下巷道环境感知数据进行融合定位，并提取视觉特征信息和激光雷达点云信息，融合井下无人车高精度定位信息，采用基于视觉词袋和概率地图的SLAM方法，将视觉特征和激光雷达数据分别转换为视觉词袋和概率数据，通过BoW模型构建SLAM高精度三维地图。

进一步的，所述步骤S204中井下无人车从当前位置到目标点的目标路径信息获取方法为：根据构建的SLAM高精度三维地图，井下无人车获取了巷道坡度、宽度和长度信息；同时利用激光雷达和视觉相机对周围环境进行感知，实时获取巷道周围环境的障碍物信息；同时，遵守保持安全距离、避免与障碍物碰撞的约束条件，朝着最短路径、最小成本和最大效率优化目标原则进行全局最优路径规划；在全局路径规划基础上，井下无人车根据实时障碍物信息进行局部调整，以生成井下无人车从当前位置到目标点的全局目标路径；根据给定目标点、自身姿态信息和SLAM高精度三维地图进行全局路径规划，获取井下无人车的全局路径，进而根据全局路径中实时障碍物信息进行局部调整，以生成井下无人车的目标路径。

进一步的，所述步骤S205中控制井下无人车底盘按照目标路径进行移动的方法为：将全局路径转换为车辆局部坐标系下的局部路径，把全局路径划分为一系列线段，计算每个线段的曲率、切向角和横向误差数据，并将其转化到车辆局部坐标系下，生成局部路径；利用井下无人车激光雷达、深度相机、IMU、霍尔编码器的传感器测量值，计算车辆的位置、速度和方向状态信息，对传感器数据进行卡尔曼滤波；根据车辆状态估计值和局部路径信息，计算车辆的控制命令；根据控制命令实际控制车辆行驶，以保持车辆在规划路径上行驶；在行驶过程中进行路径校正，路径校正根据车辆当前位置和局部路径信息，计算车辆需要进行的偏差修正值，并通过控制命令进行实时调整；通过实时状态估计、车辆控制和路径校正操作，不断反馈状态误差、路径误差和控制响应信息，对控制算法进行优化和调整，以实现更好的路径跟踪效果；对井下无人车的姿态信息、位置信息和目标路径进行PID控制算法处理，获取井下无人车的位姿控制量，进而根据位姿控制量生成驱动转向指令，以控制井下无人车按照目标路径移动。

进一步的，所述步骤S203中多源里程计信息融合获取车辆高精度姿态信息方法为：

激光里程计的计算方法为：

式中，v_x,s和v_y,s为激光雷达的速度，ω_s为激光雷达的角度，θ为扫描点的极坐标角度，r为扫描点的极坐标直径，N为扫描的大小，FOV为为扫描仪的视野，t为时间，α为扫描坐标；

视觉里程计的计算方法为：

zp＝C(RX+T)，

表示z为p的深度值，为相机平面的一个像素，位于照片中的[u,v]位置,其中f_x,f_y为相机焦距,c_x,c_y为主点的坐标,X＝[x y z]是一个三维空间点，R和T是带估计的旋转矩阵和平移向量；

IMU里程计的计算方法为：

式中x0、y0、z0分别为：初始俯仰角，初始滚转角，初始偏航角，a_x、a_y、a_z为加速度计的三轴数据，s、c分别表示sin函数和cos函数，分别表示磁力计三轴数据，分别表示磁场数据在导航下的投影数据；

轮式里程计的计算方法为：

V_L＝ω*(L+D)＝ω*(R+d)＝V+ωd，V_R＝ω*L＝ω*(R-d)＝V-ωd

V为井下无人车的线速度、ω为角速度，V_L和V_R表示左右轮速，D表示轮间距，D＝2d，L为右轮到旋转中心的距离；

Vx＝V^*cos(θ_t-1)，Vy＝V^*sin(θ_t-1)，ΔX＝Vx^*Δt^*cos(θ_t-1)，ΔY＝Vy^*Δt^*sin(θ_t-1)，

在一个时刻△t内，认为井下无人车是匀速运动，并且根据上一时刻井下无人车的航向角计算得出井下无人车在该时刻内世界坐标系上X和Y轴的增量，然后将增量进行累加处理；

x′＝x+Δx，y′＝y+Δy，θ′＝θ_t-1+w*Δt，

已知t-1时刻井下无人车位姿状态(x,y,θ),V_X,V_y,Δt,计算t时刻位姿(x',y',θ')；

多源里程计信息融合获取车辆高精度姿态信息的计算方法为：

P_k∣k＝(I-K_kH_k)P_k∣k-1

为k-1时刻对k时刻的状态预测，/>为在时刻k-1的状态估计，/>为在时刻k的状态估计，/>即为高精度姿态信息，F_k为状态转移矩阵，P_k-1∣k-1为k-1时刻的后验估计误差协方差矩阵，P_k∣k-1为k-1时刻到k时刻的估计误差协方差矩阵，Q_k为过程噪声协方差矩阵，K_k为最优卡尔曼增益，R_k为输入的多源里程信息，R_k＝[v_x,s,v_y,s,ω_s,R,T,x0,y0,z0,x',y',θ']，H_k为量测模型的雅可比矩阵，I是单位矩阵。

进一步的，所述步骤S203中构建SLAM高精度三维地图的方法为：

特征提取：对于视觉相机的图像数据，采用ORB特征算法进行特征提取；对于激光雷达数据，采用点云特征提取方法提取；

通过ORB特征算法提取图像数据信息的计算过程为：

所有特征点可可构成一个300k*M的二进制矩阵Q，其中

对激光点云信息进行特征提取的计算方法为：

S＝10,为与点云q呈连续排列的点云数,均匀分布在点云q的左右两侧，左右各5个，式中，为点云q的坐标，/>为左右相邻点的坐标，c为点云的曲率计算；

完成点云曲率的计算后，再进行点云特征提取，式中，分子为2个向量的积，即为2个向量构成的平行四边形的面积，分母为平行四边形的底边，结果即为点i到线jl的距离，计算出距离之后，保留满足条件的特征匹配点；

地图构建：对于提取的视觉特征信息，融合井下无人车的高精度姿态信息，采用基于视觉词袋的SLAM方法，将视觉特征转换为视觉词袋，并利用BoW模型进行建图；对于提取的激光雷达点云信息，融合井下无人车的高精度姿态信息，采用基于概率地图的SLAM方法，将激光雷达数据转换为概率地图，用于环境建模和定位；

将视觉特征信息、激光点云信息融合高精度姿态信息的三维地图构建过程为：

式中，L_t为当前位姿节点，S_t表示t时刻激光点云信息和视觉特征信息，观测模型P(L_t∣S_t)可以通过似然函数l(S_t＝j∣L_t)＝P(L_t∣S_t＝j)计算得到，归一化的观测模型P(S_t∣L^t)对输入的节点信息进行回环检测与图优化，连接成全局三维地图输出。

进一步的，所述步骤S204中对规划路径进行优化的方法为：

井下无人车根据建好的SLAM高精度三维地图，在三维地图上进行路径搜索，通过A*算法找到井下无人车从当前位置到终点的最短路径；

A*算法路径规划方法为：

F(s)＝G(s)+H(s)

式中，F(s)为起始点至目标点的预估消耗；G(s)为起始点至当前节点的实际消耗；H(s)为当前节点至目标点的估计消耗；

基于井下无人车在三维地图上规划的全局路径，使用Bezier曲线对全局路径进行拟合，提高路径的平滑度；

Bezier曲线拟合方法为：

p_x＝(1-t)²p₀+2t(1-t)p₁+t²p₂

上式表示在平面内选择P0、P1、P2三个不共线的点，并连接P0P1、P1P2；分别在P0P1、P1P2段选取点Pi、Pj，P0、P2两个端点不动，连接线段PiPj；在线段PiPj上选择点Px，有比值P0Pi/PiP1＝P1Pj/PjP2＝PiPx/PxPj；引入t，且t取值在0与1之间，令上述比值为t/(1-t)；

在进行路径规划时，需要对全局路径进行膨胀处理，使得车辆可以安全地通过这些障碍物或限制区域，膨胀处理方法为：

上式表示用结构B膨胀A，将结构元素B的原点平移到图像像元(x,y)位置，如果B在图像像元(x,y)处与A的交集不为空，也就是B中为1的元素位置上对应A的图像值至少有一个为1，则输出图像对应的像元(x,y)赋值为1，否则赋值为0。

与现有技术相比，本发明具有以下优点：

定位精度高，将多个传感器的里程计数据融合，可以消除各个传感器的误差和漂移，以提高定位的精度和准确性，从而提供更准确的车辆定位结果。

鲁棒性强：单一传感器可能受到环境条件的限制或噪声的干扰概率大，使用多个传感器的里程计数据融合可以增强系统的鲁棒性，当一个传感器受到异常情况的影响时，其他传感器可以提供冗余信息，帮助维持定位的稳定性和准确性。

解决环境变化问题：井下无人车在实际运行过程中会遇到环境变化，如不同的路面条件、变化的动态障碍物等；本发明通过融合多个传感器的数据，在不同环境中提供稳定和一致的定位结果，根据实时环境变化，可以动态地选择和调整传感器权重，以适应不同的场景和条件。

增加安全性：井下无人车在高速行驶或复杂道路环境中需要更高的安全性，本发明可以提供更准确的位姿信息，帮助井下无人车在实时感知周围环境和进行精确的障碍物避障，通过融合多个传感器的数据，可以提高对车辆和周围环境的感知能力，从而增加安全性和可靠性。

附图说明

图1为本发明流程图；

图2为本发明导航装置结构布置示意图；

图3为本发明无人车底盘结构示意图；

图中：101、上层控制单元，102、底层控制单元，103、无人车底盘，104、IMU，105、深度相机传感器，106、激光雷达，107、输入接口，108、输出接口，109、远程PC，110、无线路由器，111、无线网卡，112、主控制器，113、车载控制器，114、转向电机控制器，115、驱动电机控制器，116、轮式底盘，117、悬架，118、霍尔编码器，119减速电机。

具体实施方式

下面结合附图对本发明作进一步说明。

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

如图1所示，本发明包括以下步骤：

S201：根据井下无人车的外形结构，布置导航装置；导航装置包括上层控制单元101、底层控制单元102和无人车底盘103；

如图2所示，上层控制单元101包括IMU104、深度相机传感器105、激光雷达106、远程PC109、无线网卡111和主控制器112，IMU104、深度相机传感器105、激光雷达106与主控制器112连接，无线网卡111设置在主控制器112中，远程PC109通过无线路由器110和主控制器112连接；

底层控制单元102包括车载控制器113、转向电机控制器114和驱动电机控制器115，转向电机控制器115、驱动电机控制器114与车载控制器113通过can接口连接，主控制器112与车载控制器113通过can接口连接；

如图3所示，无人车底盘103包括轮式底盘116、霍尔编码器118、减速电机119和悬架117，减速电机119内置于井下无人车车轮中，霍尔编码器118与减速电机119连接，左右两个悬架117下端采用螺栓连接的方式与轮式底盘116固定、上端通过顶部支撑与轮式底盘116连接。

通过上层控制单元101中与主控制器112连接的激光雷达106、深度相机传感器105对环境数据进行采集，获取环境感知数据，连接的IMU104实现对无人车速度、加速度和角速度信息提取。主控制器112的输入接口107和输出接口108采用高速接口，可以通过通用串行总线、网线、蓝牙或者串口等进行输入输出。输入接口107可以连接鼠标和键盘，进行自主导航参数设定。输出接口108可以与液晶显示屏设备连接，输出定位、建图、路径规划和运动控制等自主导航实时数据，便于用户查看自主导航的可视化数据。主控制器112中插入无线网卡111，通过无线路由器110和远程PC(个人计算机)109连接，实现远程可视化显示。在主控制器112中，安装了机器人操作系统ROS，且ROS中内置了启动器，其是ROS中启动多个节点的命令，通过启动器启动文件，可以提高工作效率，降低操作难度。车载控制器113采集减速电机119的真实反馈值，并通过转向电机控制器115和驱动电机控制器114控制电机的转角与速度。

S202：对导航装置进行联合参数标定；在激光雷达中，标定可以解决激光束旋转中心不在井下无人车轴心上的问题；在深度相机中，标定可以解决畸变问题，调整相机内部参数和外部参数，提高深度估计的精度；在轮式里程计中，由于轮胎的滑动、弹性变形等因素，其存在累积误差的问题，标定可以消除这些误差，提高车辆自身运动状态的测量精度；在IMU中，标定可以消除积分漂移带来的误差。

S203：井下无人车根据多类型数据采集装置进行数据采集，获取井下无人车多类型里程计信息和多类型井下巷道环境感知信息，多类型里程计分别为激光里程计、视觉里程计、轮式里程计和IMU里程计，将多源里程计信息融合获取车辆高精度姿态信息；

激光里程计是一种基于激光雷达数据的井下无人车姿态估计技术。具体来说，激光雷达发出一束激光，然后测量这束激光反射回来所需的时间，从而确定它与地面或物体的距离，并通过多次扫描并取得不同角度下的数据，获取激光点云的三维坐标信息；接着，对激光点云信息进行高斯滤波，去除掉噪声和无效的点云数据；最后，将激光点云信息进行迭代最近点配准，以获取激光雷达在运动过程中的姿态信息。

激光里程计的计算方法为：

将每个扫描点的角度和坐标带入上式，即可以求解出v_x,s和v_y,s，式中，v_x,s和v_y,s为激光雷达的速度，ω_s为激光雷达的角度，θ为扫描点的极坐标角度，r为扫描点的极坐标直径，N为扫描的大小，FOV为为扫描仪的视野，/>t为时间，α为扫描坐标。

视觉里程计是一种利用相机和计算机视觉技术来确定移动井下无人车的三维姿态和位置的方法。获取视觉里程计的方式通常是利用车载相机捕获连续的图像，然后通过计算这些图像之间的位移来估算车辆在运行过程中的位置和姿态。具体而言，视觉里程计首先会从相邻两帧图像中提取出特征点，比如角点等。接着，通过匹配这些特征点，可以得到两个时间戳之间相机的位移向量。不断重复这个过程，就可以得到整个运动过程中车辆的轨迹。

视觉里程计的计算方法为：

zp＝C(RX+T)，

表示z为p的深度值，为相机平面的一个像素，位于照片中的[u,v]位置,其中f_x,f_y为相机焦距,c_x,c_y为主点的坐标,X＝[x y z]是一个三维空间点，R和T是带估计的旋转矩阵和平移向量。

惯性测量单元即IMU里程计，记录车辆在三个轴向上的加速度和角速度信息。这些数据可以用来计算车辆在三个轴向上的位移、速度和姿态信息。IMU里程计是利用IMU传感器在车辆运动过程中采集的运动信息来推算车辆运动状况，进而实现对车辆位置和姿态的推测，从而实现高精度和持续的车辆定位。IMU里程计主要目的是测量井下无人车的姿态角，姿态测量原理为，通过陀螺仪中的角速度测量，可以实时地测量车辆在三个轴向上的旋转状态，这种旋转状态被称为车辆的姿态角。根据角度变化率对转角进行测量，然后将测得的数据进行离散化，就可以得到姿态角数据。

IMU里程计的计算方法为：

式中x0、y0、z0分别为：初始俯仰角，初始滚转角，初始偏航角，a_x、a_y、a_z为加速度计的三轴数据，s、c分别表示sin函数和cos函数。分别表示磁力计三轴数据，分别表示磁场数据在导航下的投影数据。

轮式里程计是一种测量车辆位移的设备，它使用车轮旋转的原理来计算车辆在水平方向上的运动距离。它的工作原理基于车轮旋转的速度与车辆行驶距离之间的关系。通常情况下，车轮周围安装有霍尔编码器，通过检测车轮旋转的次数以及每次旋转所对应的距离，可以计算出车辆行驶的距离。轮式里程计的实现过程主要涉及到以下步骤：将霍尔编码器安装在车轮周围，通常是通过螺丝固定在车轮旋转轴上；传感器会以一定频率采集车轮旋转的信息，包括旋转次数和每次旋转的角度等参数；将采集到的数据进行处理，计算车轮的旋转速度和行驶距离；将每次计算出的行驶距离累加起来，就可以得到车辆行驶的位移信息，对井下无人车的位移信息进行处理可实现井下无人车在空间坐标系中的位置估计。

轮式里程计的计算方法为：

V_L＝ω*(L+D)＝ω*(R+d)＝V+ωd，V_R＝ω*L＝ω*(R-d)＝V-ωd

V为井下无人车的线速度、ω为角速度，V_L和V_R表示左右轮速，D表示轮间距，D＝2d，L为右轮到旋转中心的距离；

Vx＝V^*cos(θ_t-1)，Vy＝V^*sin(θ_t-1)，ΔX＝Vx^*Δt^*cos(θ_t-1)，ΔY＝Vy^*Δt^*sin(θ_t-1)，

在一个短的时刻△t内，认为井下无人车是匀速运动，并且根据上一时刻井下无人车的航向角计算得出井下无人车在该时刻内世界坐标系上X和Y轴的增量，然后将增量进行累加处理；

x′＝x+Δx，y′＝y+Δy，θ′＝θ_t-1+w*Δt，

已知t-1时刻井下无人车位姿状态(x,y,θ),V_X,V_y,Δt,计算t时刻位姿(x',y',θ')。

对于每一时刻，将井下无人车的激光里程计、轮式里程计、IMU里程计和视觉里程计进行时间戳同步，且把上述得到的位姿变化信息按照一定的加权系数进行融合，得到整个井下无人车的融合高精度姿态信息。多源里程信息融合的具体实现步骤如下：按照不同类型里程计信息的特点，确定每种里程计所对应的状态变量和测量变量；对每个状态变量随机生成一组粒子，并利用状态转移模型将粒子进行预测；根据每个里程计的测量值，计算每个粒子对应的权值，并进行重采样；为每个传感器分配不同的权重，并将不同传感器提供的数据进行加权平均，从而得到高精度的井下无人车姿态信息。多源里程计信息融合获取车辆高精度姿态信息的计算方法为：

P_k∣k＝(I-K_kH_k)P_k∣k-1

为k-1时刻对k时刻的状态预测，/>为在时刻k-1的状态估计，/>为在时刻k的状态估计，/>即为高精度姿态信息，F_k为状态转移矩阵，P_k-1∣k-1为k-1时刻的后验估计误差协方差矩阵，P_k∣k-1为k-1时刻到k时刻的估计误差协方差矩阵，Q_k为过程噪声协方差矩阵，K_k为最优卡尔曼增益，R_k为输入的多源里程信息，R_k＝[v_x,s,v_y,s,ω_s,R,T,x0,y0,z0,x',y',θ']，H_k为量测模型的雅可比矩阵，I是单位矩阵。

井下无人车根据自身高精度姿态信息和多类型井下巷道环境感知数据进行融合定位，并提取视觉特征信息和激光雷达点云信息，融合井下无人车高精度定位信息，采用基于视觉词袋和概率地图的SLAM方法，将视觉特征和激光雷达数据分别转换为视觉词袋和概率数据，通过BoW模型构建SLAM高精度三维地图，构建SLAM高精度三维地图的方法为：

特征提取：对于视觉相机的图像数据，采用ORB特征算法进行特征提取，ORB特征算法是一种基于FAST角点检测和BRIEF二进制描述子的特征提取方法，具有旋转不变性和尺度不变性，其可以提取出图像中的关键点和描述子；对于激光雷达数据，采用点云特征提取方法提取，点云特征提取方法是一种基于几何形状的点云特征提取方法，可以提取出点云中的关键点、法向量、曲率等特征信息；

通过ORB特征算法提取图像数据关键信息的计算过程为：

所有特征点可可构成一个300k*M的二进制矩阵Q，其中

对激光点云信息进行特征提取的计算方法为：

S＝10,为与点云q呈连续排列的点云数,均匀分布在点云q的左右两侧，左右各5个，式中，为点云q的坐标，/>为左右相邻点的坐标，c为点云的曲率计算；

完成点云曲率的计算后，再进行点云特征提取，式中，分子为2个向量的积，即为2个向量构成的平行四边形的面积，分母为平行四边形的底边，结果即为点i到线jl的距离，计算出距离之后，如果距离足够小，保留满足条件的特征匹配点；

地图构建：对于提取的视觉特征信息，融合井下无人车的高精度姿态信息，采用基于视觉词袋的SLAM方法，将视觉特征转换为视觉词袋，并利用BoW模型进行建图；对于提取的激光雷达点云信息，融合井下无人车的高精度姿态信息，采用基于概率地图的SLAM方法，将激光雷达数据转换为概率地图，用于环境建模和定位；

将视觉特征信息、激光点云信息融合高精度姿态信息的三维地图构建过程为：

式中，L_t为当前位姿节点，S_t表示t时刻激光点云信息和视觉特征信息，观测模型P(L_t∣S_t)可以通过似然函数l(S_t＝j∣L_t)＝P(L_t∣S_t＝j)计算得到，归一化的观测模型P(S_t∣L^t)对输入的节点信息进行回环检测与图优化，连接成全局三维地图输出；

准确定位：根据井下无人车当前位置信息和高精度的姿态信息，可以实现井下无人车在巷道环境中的定位。具体来说，利用井下无人车高精度姿态信息和地图信息，采用匹配算法进行井下无人车定位。匹配算法可以将井下无人车当前的传感器数据与地图信息进行比较，找到与当前传感器数据最匹配的地图区域，从而确定井下无人车的位置和姿态。井下无人车采用基于信息熵的匹配算法，可以实现高效、准确的井下巷道环境定位。

S204：井下无人车根据给定目标点、自身姿态信息和SLAM三维地图进行路径规划，获取从当前位置到目标点的目标路径信息；且对规划路径进行优化，使其更符合车辆运动学原理；目标路径信息获取方法为：根据构建的SLAM高精度三维地图，井下无人车获取了巷道坡度、宽度和长度信息；同时利用激光雷达和视觉相机对周围环境进行感知，实时获取巷道周围环境的障碍物信息；同时，遵守保持安全距离、避免与障碍物碰撞的约束条件，朝着最短路径、最小成本和最大效率优化目标原则进行全局最优路径规划；在全局路径规划基础上，井下无人车根据实时障碍物信息进行局部调整，以生成井下无人车从当前位置到目标点的全局目标路径。根据给定目标点、自身姿态信息和SLAM三维地图进行全局路径规划，获取井下无人车的全局路径，进而根据全局路径中实时障碍物信息进行局部调整，以生成井下无人车的目标路径。

对规划路径进行优化的方法为：

井下无人车根据建好的SLAM高精度三维地图，在三维地图上进行路径搜索，通过A*算法找到井下无人车从当前位置到终点的最短路径；

A*算法路径规划方法为：

F(s)＝G(s)+H(s)

式中，F(s)为起始点至目标点的预估消耗；G(s)为起始点至当前节点的实际消耗；H(s)为当前节点至目标点的估计消耗；A*算法原理可以简单描述为以下几个步骤：首先，在地图上设置初始位置点和目标位置点；然后，A*算法从初始位置点开始朝着目标位置点有方向地进行搜索，寻找离当前节点F(s)最小的节点；一旦找到了这个节点，它将作为下一个基础点，再次寻找距离该基础点F(s)值的最小点；这个过程会一直循环，直至找到目标位置点。

基于井下无人车在三维地图上规划的全局路径，使用Bezier曲线对全局路径进行拟合，通过Bezier曲线的平滑性质，可以将路径的曲率减小，提高路径的平滑度；

Bezier曲线拟合方法为：

p_x＝(1-t)²p₀+2t(1-t)p₁+t²p₂

上式表示在平面内选择P0、P1、P2三个不共线的点，并连接P0P1、P1P2；分别在P0P1、P1P2段选取点Pi、Pj，P0、P2两个端点不动，连接线段PiPj；在线段PiPj上选择点Px，有比值P0Pi/PiP1＝P1Pj/PjP2＝PiPx/PxPj；引入t，且t取值在0与1之间，令上述比值为t/(1-t)；

井下无人车行驶过程中，需要遵循规定的路径进行行驶，而在巷道现实环境中，道路上可能存在许多障碍物或限制区域。因此，在进行路径规划时，需要对全局路径进行膨胀处理，使得车辆可以安全地通过这些障碍物或限制区域。因此，对路径过于靠近障碍物的位置，通过膨胀函数对路径进行膨胀，全局路径上的每个点向两侧分别偏移一定的距离，生成新的平滑路径，同时将井下无人车行进过程中的尺寸考虑进去，从而在行驶过程中保持安全距离，以便保证井下无人车的安全通过；

膨胀函数对全局路径的处理方法为：

上式表示用结构B膨胀A，将结构元素B的原点平移到图像像元(x,y)位置，如果B在图像像元(x,y)处与A的交集不为空，也就是B中为1的元素位置上对应A的图像值至少有一个为1，则输出图像对应的像元(x,y)赋值为1，否则赋值为0。

粗略地说，膨胀会使目标区域范围“变大”，将于目标区域接触的背景点合并到该目标物中，使目标边界向外部扩张。作用就是可以用来填补目标区域中某些空洞以及消除包含在目标区域中的小颗粒噪声。

S205：井下无人车根据自身姿态信息、位置信息和目标路径信息生成导航控制指令，以控制其底盘按照目标路径进行移动；移动方法为：将全局路径转换为车辆局部坐标系下的局部路径，把全局路径划分为一系列线段，计算每个线段的曲率、切向角和横向误差数据，并将其转化到车辆局部坐标系下，生成局部路径；利用井下无人车激光雷达、深度相机、IMU、霍尔编码器的传感器测量值，计算车辆的位置、速度和方向状态信息，对传感器数据进行卡尔曼滤波，去除误差较大的数据，提高车辆的位置、速度和方向等状态信息精度；根据车辆状态估计值和局部路径信息，计算车辆的控制命令，控制命令包括速度、转向角度和刹车控制信息，速度控制可以根据局部路径的曲率和车辆的动能进行调整，以保证车辆稳定行驶，转向控制则需要考虑路径跟踪误差和车辆转向响应时间等因素，对转向角度进行实时调整；根据控制命令实际控制车辆行驶，例如控制电机转速、转向电机开度、制动器力度等，以保持车辆在规划路径上行驶；在行驶过程中，由于环境变化和传感器误差因素，井下无人车可能会发生偏离规划路径的情况，此时需要进行路径校正，路径校正根据车辆当前位置和局部路径信息，计算车辆需要进行的偏差修正值，并通过控制命令进行实时调整；通过实时状态估计、车辆控制和路径校正操作，不断反馈状态误差、路径误差和控制响应信息，对控制算法进行优化和调整，以实现更好的路径跟踪效果。对井下无人车的姿态信息、位置信息和目标路径进行PID控制算法处理，获取井下无人车的位姿控制量，进而根据位姿控制量生成驱动转向指令，以控制井下无人车按照目标路径移动。

在井下无人驾驶无轨胶轮车上安装16线激光雷达、RGBD深度相机、霍尔编码器和IMU传感器，并通过对上述传感器采集的巷道点云信息、深度图像信息、车辆转速转角信息和加减速度信息进行处理分别得到激光里程计、视觉里程计、轮式里程计和IMU里程计。

利用卡尔曼滤波对上诉多种类型的里程计信息进行融合，得到车辆高精度姿态信息，该姿态信息相对于未融合前的姿态信息提高了20％的精度；并将车辆高精度姿态信息结合激光雷达的点云信息和深度相机的图像信息，构建井下三维环境地图，该井下地图长50m,宽40m，且车道宽为5m，其还包含了井下巷道壁和躲避硐室等重要信息，与实际巷道环境相比，还原精度高达95％，为无人车的自主导航提供了重要环境依据。

在构建好的三维环境地图上，无人车展开初始定位，定位坐标为(0，0)，同时对给定的目标点(40，35)进行路径规划，利用Bezier曲线对全局路径进行优化，优化后的全局路径转弯处转弯半径范围为2.5m到3.5m，且使用膨胀函数处理全局路径，处理后的全局路径与巷道壁最小距离为1m。

井下无人车根据自身高精度姿态信息、自身位置信息和目标路径信息进行PID控制算法处理，获取井下无人车的位姿控制量，其最大前进速度为15km/h，最大转向速度为3rad/s，并根据位姿控制量生成驱动转向指令，以控制其底盘按照目标路径进行移动；无人车在路径跟踪过程始终与巷道壁保持至少0.5m的安全距离，且在转弯处前进速度不超过5km/h，转向角速度不超过2rad/s，若在车辆前方8m内遇到障碍物，无人车立刻减速至3km/h，并在障碍物侧面0.5m外局部路径规划绕行躲避障碍物，同时路径跟踪误差低于3％，其在抵达目标点(40，35)后停止行驶，完成导航目标任务。

对于本领域技术人员而言，显然本发明不限于上述示范性实施例的细节，而且在不背离本发明的精神或基本特征的情况下，能够以其它的具体形式实现本发明。因此，无论从哪一点来看，均应将实施例看作是示范性的，而且是非限制性的，本发明的范围由所附权利要求而不是上述说明限定，因此旨在将落在权利要求的等同要件的含义和范围内的所有变化囊括在本发明内。不应将权利要求中的任何附图标记视为限制所涉及的权利要求。

以上所述，仅为本发明的较佳实施例，并不用以限制本发明，凡是依据本发明的技术实质对以上实施例所作的任何细微修改、等同替换和改进，均应包含在本发明技术方案的保护范围之内。

Claims (8)

1.一种多源里程融合SLAM井下导航方法，其特征在于，包括以下步骤：

S201：根据井下无人车的外形结构，布置导航装置；

S202：对导航装置进行联合参数标定；

S203：井下无人车根据多类型数据采集装置进行数据采集，获取井下无人车多类型里程计信息和多类型井下巷道环境感知信息，将多源里程计信息融合获取车辆高精度姿态信息，井下无人车根据自身高精度姿态信息和多类型井下巷道环境感知数据进行融合定位，并构建SLAM高精度三维地图；

S204：井下无人车根据给定目标点、自身姿态信息和SLAM三维地图进行路径规划，获取从当前位置到目标点的目标路径信息，且对规划路径进行优化；

S205：井下无人车根据自身姿态信息、位置信息和目标路径信息生成导航控制指令，以控制其底盘按照目标路径进行移动。

2.根据权利要求1所述的一种多源里程融合SLAM井下导航方法，其特征在于，所述步骤S201中的导航装置包括上层控制单元(101)、底层控制单元(102)和无人车底盘(103)；

上层控制单元(101)包括IMU(104)、深度相机传感器(105)、激光雷达(106)、远程PC(109)、无线网卡(111)和主控制器(112)，IMU(104)、深度相机传感器(105)、激光雷达(106)与主控制器(112)连接，无线网卡(111)设置在主控制器(112)中，远程PC(109)通过无线路由器(110)和主控制器(112)连接；

底层控制单元(102)包括车载控制器(113)、转向电机控制器(114)和驱动电机控制器(115)，转向电机控制器(115)、驱动电机控制器(114)与车载控制器(113)通过can接口连接，主控制器(112)与车载控制器(113)通过can接口连接；

无人车底盘(103)包括轮式底盘(116)、霍尔编码器(118)、减速电机(119)和悬架(117)，减速电机(119)内置于井下无人车车轮中，霍尔编码器(118)与减速电机(119)连接，左右两个悬架(117)的上端和下端均与轮式底盘(116)连接。

3.根据权利要求1所述的一种多源里程融合SLAM井下导航方法，其特征在于，所述步骤S203中多类型里程计分别为激光里程计、视觉里程计、轮式里程计和IMU里程计；井下无人车根据自身高精度姿态信息和多类型井下巷道环境感知数据进行融合定位，并提取视觉特征信息和激光雷达点云信息，融合井下无人车高精度定位信息，采用基于视觉词袋和概率地图的SLAM方法，将视觉特征和激光雷达数据分别转换为视觉词袋和概率数据，通过BoW模型构建SLAM高精度三维地图。

4.根据权利要求1所述的一种多源里程融合SLAM井下导航方法，其特征在于，所述步骤S204中井下无人车从当前位置到目标点的目标路径信息获取方法为：根据构建的SLAM高精度三维地图，井下无人车获取了巷道坡度、宽度和长度信息；同时利用激光雷达和视觉相机对周围环境进行感知，实时获取巷道周围环境的障碍物信息；同时，遵守保持安全距离、避免与障碍物碰撞的约束条件，朝着最短路径、最小成本和最大效率优化目标原则进行全局最优路径规划；在全局路径规划基础上，井下无人车根据实时障碍物信息进行局部调整，以生成井下无人车从当前位置到目标点的全局目标路径；根据给定目标点、自身姿态信息和SLAM高精度三维地图进行全局路径规划，获取井下无人车的全局路径，进而根据全局路径中实时障碍物信息进行局部调整，以生成井下无人车的目标路径。

5.根据权利要求1所述的一种多源里程融合SLAM井下导航方法，其特征在于，所述步骤S205中控制井下无人车底盘按照目标路径进行移动的方法为：将全局路径转换为车辆局部坐标系下的局部路径，把全局路径划分为一系列线段，计算每个线段的曲率、切向角和横向误差数据，并将其转化到车辆局部坐标系下，生成局部路径；利用井下无人车激光雷达、深度相机、IMU、霍尔编码器的传感器测量值，计算车辆的位置、速度和方向状态信息，对传感器数据进行卡尔曼滤波；根据车辆状态估计值和局部路径信息，计算车辆的控制命令；根据控制命令实际控制车辆行驶，以保持车辆在规划路径上行驶；在行驶过程中进行路径校正，路径校正根据车辆当前位置和局部路径信息，计算车辆需要进行的偏差修正值，并通过控制命令进行实时调整；通过实时状态估计、车辆控制和路径校正操作，不断反馈状态误差、路径误差和控制响应信息，对控制算法进行优化和调整，以实现更好的路径跟踪效果；对井下无人车的姿态信息、位置信息和目标路径进行PID控制算法处理，获取井下无人车的位姿控制量，进而根据位姿控制量生成驱动转向指令，以控制井下无人车按照目标路径移动。

6.根据权利要求1所述的一种多源里程融合SLAM井下导航方法，其特征在于，所述步骤S203中多源里程计信息融合获取车辆高精度姿态信息方法为：

激光里程计的计算方法为：

式中，v_x,s和v_y,s为激光雷达的速度，ω_s为激光雷达的角度，θ为扫描点的极坐标角度，r为扫描点的极坐标直径，N为扫描的大小，FOV为为扫描仪的视野，t为时间，α为扫描坐标；

视觉里程计的计算方法为：

zp＝C(RX+T)，

表示z为p的深度值，为相机平面的一个像素，位于照片中的[u,v]位置,其中f_x,f_y为相机焦距,c_x,c_y为主点的坐标,X＝[x y z]是一个三维空间点，R和T是带估计的旋转矩阵和平移向量；

IMU里程计的计算方法为：

式中x0、y0、z0分别为：初始俯仰角，初始滚转角，初始偏航角，a_x、a_y、a_z为加速度计的三轴数据，s、c分别表示sin函数和cos函数，分别表示磁力计三轴数据，/>分别表示磁场数据在导航下的投影数据；

轮式里程计的计算方法为：

V_L＝ω*(L+D)＝ω*(R+d)＝V+ωd，V_R＝ω*L＝ω*(R-d)＝V-ωd

V为井下无人车的线速度、ω为角速度，V_L和V_R表示左右轮速，D表示轮间距，D＝2d，L为右轮到旋转中心的距离；

Vx＝V^*cos(θ_t-1)，Vy＝V^*sin(θ_t-1)，ΔX＝Vx^*Δt^*cos(θ_t-1)，ΔY＝Vy^*Δt^*sin(θ_t-1)，

在一个时刻△t内，认为井下无人车是匀速运动，并且根据上一时刻井下无人车的航向角计算得出井下无人车在该时刻内世界坐标系上X和Y轴的增量，然后将增量进行累加处理；

x′＝x+Δx，y′＝y+Δy，θ′＝θ_t-1+w*Δt，

已知t-1时刻井下无人车位姿状态(x,y,θ),V_X,V_y,Δt,计算t时刻位姿(x',y',θ')；

多源里程计信息融合获取车辆高精度姿态信息的计算方法为：

P_k∣k＝(I-K_kH_k)P_k∣k-1

为k-1时刻对k时刻的状态预测，/>为在时刻k-1的状态估计，/>为在时刻k的状态估计，/>即为高精度姿态信息，F_k为状态转移矩阵，P_k-1∣k-1为k-1时刻的后验估计误差协方差矩阵，P_k∣k-1为k-1时刻到k时刻的估计误差协方差矩阵，Q_k为过程噪声协方差矩阵，K_k为最优卡尔曼增益，R_k为输入的多源里程信息，R_k＝[v_x,s,v_y,s,ω_s,R,T,x0,y0,z0,x',y',θ']，H_k为量测模型的雅可比矩阵，I是单位矩阵。

7.根据权利要求3所述的一种多源里程融合SLAM井下导航方法，其特征在于，所述步骤S203中构建SLAM高精度三维地图的方法为：

特征提取：对于视觉相机的图像数据，采用ORB特征算法进行特征提取；对于激光雷达数据，采用点云特征提取方法提取；

通过ORB特征算法提取图像数据信息的计算过程为：

所有特征点可可构成一个300k*M的二进制矩阵Q，其中

对激光点云信息进行特征提取的计算方法为：

S＝10,为与点云q呈连续排列的点云数,均匀分布在点云q的左右两侧，左右各5个，式中，为点云q的坐标，/>为左右相邻点的坐标，c为点云的曲率计算；

完成点云曲率的计算后，再进行点云特征提取，式中，分子为2个向量的积，即为2个向量构成的平行四边形的面积，分母为平行四边形的底边，结果即为点i到线jl的距离，计算出距离之后，保留满足条件的特征匹配点；

地图构建：对于提取的视觉特征信息，融合井下无人车的高精度姿态信息，采用基于视觉词袋的SLAM方法，将视觉特征转换为视觉词袋，并利用BoW模型进行建图；对于提取的激光雷达点云信息，融合井下无人车的高精度姿态信息，采用基于概率地图的SLAM方法，将激光雷达数据转换为概率地图，用于环境建模和定位；

将视觉特征信息、激光点云信息融合高精度姿态信息的三维地图构建过程为：

式中，L_t为当前位姿节点，S_t表示t时刻激光点云信息和视觉特征信息，观测模型P(L_t∣S_t)可以通过似然函数计算得到，归一化的观测模型P(S_t∣L^t)对输入的节点信息进行回环检测与图优化，连接成全局三维地图输出。

8.根据权利要求1所述的一种多源里程融合SLAM井下导航方法，其特征在于，所述步骤S204中对规划路径进行优化的方法为：

井下无人车根据建好的SLAM高精度三维地图，在三维地图上进行路径搜索，通过A*算法找到井下无人车从当前位置到终点的最短路径；

A*算法路径规划方法为：

F(s)＝G(s)+H(s)

式中，F(s)为起始点至目标点的预估消耗；G(s)为起始点至当前节点的实际消耗；H(s)为当前节点至目标点的估计消耗；

基于井下无人车在三维地图上规划的全局路径，使用Bezier曲线对全局路径进行拟合，提高路径的平滑度；

Bezier曲线拟合方法为：

p_x＝(1-t)²p₀+2t(1-t)p₁+t²p₂

上式表示在平面内选择P0、P1、P2三个不共线的点，并连接P0P1、P1P2；分别在P0P1、P1P2段选取点Pi、Pj，P0、P2两个端点不动，连接线段PiPj；在线段PiPj上选择点Px，有比值P0Pi/PiP1＝P1Pj/PjP2＝PiPx/PxPj；引入t，且t取值在0与1之间，令上述比值为t/(1-t)；

在进行路径规划时，需要对全局路径进行膨胀处理，使得车辆可以安全地通过这些障碍物或限制区域，膨胀处理方法为：

上式表示用结构B膨胀A，将结构元素B的原点平移到图像像元(x,y)位置，如果B在图像像元(x,y)处与A的交集不为空，也就是B中为1的元素位置上对应A的图像值至少有一个为1，则输出图像对应的像元(x,y)赋值为1，否则赋值为0。