无GPS信号下二维码、激光雷达与IMU融合定位系统及方法

无gps信号下二维码、激光雷达与imu融合定位系统及方法
技术领域
1.本发明涉及导航定位技术领域，尤其涉及无人车在矿下场景的导航定位，具体为一种无gps信号下二维码、激光雷达与imu融合定位系统及方法。


背景技术：

2.目前，室外导航定位技术已经发展的比较成熟，如全球卫星导航系统（global navigation satellite system, gnss）在室外场景中，能够提供准确的地理位置、行车速度等信息，给人们的日常生活带来极大的便利。但是在室内或地下等无gps（global position system，全球定位系统）信号环境下，由于障碍物的遮挡，gnss无法提供可靠、连续的定位服务。而随着我国对在室内和地下等无gps信号环境下的需求日益增多，如室内导航、室内移动测图系统、地下交通导航定位、地下作业安全等领域，因此，研究一种无gps信号环境下的多源传感器融合的无人车定位方法对于地下隧道开挖、矿产资源的勘探、矿山开采和事故应急等应用具有重要意义。
3.经检索，公开号cn113566833a的中国专利于2021年10月29日公开了一种多传感器融合的车辆定位方法，该方法对于可以接收gps信号的场景，则融合gps装置和imu惯性传感器进行车辆的定位，使用激光雷达进行障碍物检测，实现车辆的定位导航；对于不能接收gps信号的场景，则使用激光雷达构建栅格地图，融合imu惯性传感器和相机进行车辆的定位和障碍物检测，实现车辆的定位导航。该专利申请在无gps信号场景下，需要重新构建栅格地图、再融合多传感器数据实现定位导航，计算量较大且实时性难以满足实际需求，不适用于矿下场景。
4.公开号cn113160400a的中国专利于2021年7月23日公开了一种地下地形的定位方法，包括：获取地下空间的三维实体化地图，所述三维实体化地图中记录有地下空间的空间特征信息；车辆在所述地下空间行驶时，获取所述车辆所在环境的空间特征；将所述空间特征与所述空间特征信息进行比对，根据比对结果确定所述车辆在所述地下空间的位置。该专利申请通过开采过程中根据井下实际环境探测到的空间特征信息的比对来定位车辆的实际位置，对于有相似空间特征信息的井下路径，则存在车辆实际位置难以准确定位的问题。
5.因此，提供一种无人车定位方法，能够实现矿下场景的准确定位导航是至关重要的。


技术实现要素：

6.为克服上述现有技术的不足，本发明提供一种无gps信号下二维码、激光雷达与imu融合定位系统及方法，以解决矿下无人车辆的导航定位问题。
7.根据本发明说明书的一方面，提供一种无gps信号下二维码、激光雷达与imu融合定位系统，所述系统应用于矿下场景，所述系统包括：采集模块，用于获取路侧端的二维码图像、车道线图像、车辆的激光雷达数据及
imu数据；构建模块，用于根据车辆遍历矿下所有路径时所获取的激光雷达数据，构建矿下场景的高精地图；车辆初始位置获取模块，用于根据采集的二维码图像识别当前二维码所处目标物在高精地图上的绝对位置，并基于所述绝对位置和车辆与目标物之间的相对距离得到车辆在高精地图中的初始位置；车辆实时位置更新模块，用于在车辆行驶过程中，将车辆的激光雷达数据与imu数据进行融合，并将融合后的imu数据、车辆当前位置的激光雷达点云数据及车道线图像分别与高精地图进行匹配，在匹配成功时进行车辆实时位置的更新。
8.上述技术方案中，在初始时，车辆将矿下所有路径遍历一遍，并在遍历过程中获取车辆的激光雷达数据，该激光雷达数据经由构建模块构建形成矿下场景的高精地图；在车辆启动时，通过采集模块获取当前路侧端的二维码图像，该二维码图像经由车辆初始位置获取模块进一步处理，得到该二维码所处目标物在高精地图的绝对位置，然后利用该绝对位置及激光雷达探测到的车辆与二维码所处目标物的相对距离，得到车辆初始时的位置信息；在车辆行驶过程中，通过采集模块实时获取车辆的imu数据、激光雷达数据及车道线图像，根据imu数据解算得到导航参数，根据激光雷达数据对导航参数进行融合修正，通过修正减少或消除imu解算带来的误差累积，然后将融合修正后的导航参数、车辆当前位置的激光雷达点云数据及车道线图像分别与构建的高精地图进行匹配，若匹配成功，则更新车辆的实时位置；若匹配不成功，则重新获取融合后的数据，再进行匹配。
9.上述技术方案中，基于车辆的imu数据得到的车辆位姿信息是相对于导航坐标系，因此需要进行坐标转换后再与高精地图进行匹配。
10.作为进一步的技术方案，所述采集模块包括图像采集装置，用于获取路侧端的二维码图像及车道线图像；激光雷达，用于获取车辆的激光雷达数据以及探测车辆与目标物之间的相对距离；imu，用于获取车辆的imu数据。图像采集装置包括相机，可采用不同的相机来分别获取二维码图像和车道线图像，用于获取二维码图像的相机可设置在车身中部位置，用于获取车道线图像的相机可设置在车头位置。imu包括陀螺仪和加速度计。
11.作为进一步的技术方案，所述系统还包括联合标定模块，用于根据icp迭代最近点法对激光雷达和imu进行标定以获取激光雷达与imu之间的外部参数，以及根据在线自标定方法对图像采集装置和imu进行标定以获取图像采集装置与imu之间的外部参数。
12.根据icp迭代最近点法对激光雷达和imu进行标定的过程包括：首先选取某个标志物，由imu惯性传感器和激光雷达采集该标志物的位姿数据，得到该标志物在imu惯导坐标系下的坐标和在激光雷达坐标系下的点云坐标，然后将这两个坐标进行数据关联，得到imu惯导坐标系与激光雷达坐标系的变换矩阵。
13.根据在线自标定方法对图像采集装置（如相机）与imu进行标定的过程包括：根据某一时刻相机同时具有最大的内点数和最高的阈值幅度来选择一个主相机，以线性方式初始化相机与imu之间的旋转，并使用概率线性滑动窗口处理相机和imu的平移，然后利用高精度的非线性图优化方式去修正标定结果。此外，在标定过程中增加了终止机制来减少计算资源的消耗。
14.作为进一步的技术方案，所述二维码图像布设有多个，且多个所述二维码图像沿
矿下路径的路侧端间隔布设。可在矿下路径的路侧端布设多个路桩，将二维码图像贴附于路桩上。为提高矿下场景下采集二维码图像的清晰度，可在路桩上设置照明装置来照亮二维码。
15.进一步来说，路桩上的照明装置可由系统控制，当系统检测到二维码图像清晰度达不到预期时，控制照明装置点亮，在其他时候，照明装置则处于休眠状态，从而既能够保证二维码图像的清晰度需求，又能够节能减耗。
16.作为进一步的技术方案，所述系统还包括控制模块，用于在车辆行驶过程中，按照预设周期控制采集模块重新获取路侧端的二维码图像。该技术方案使车辆能够在行驶过程中定时获取路侧端二维码的图像，从而重新确认车辆在高精地图上的绝对位置，修正因时间累积造成的导航误差，进一步提高定位精度。
17.作为进一步的技术方案，所述车辆实时位置更新模块进一步包括：数据补偿单元，用于获取激光雷达点云数据的畸变部分并进行补偿；预处理单元，用于在激光雷达数据与imu数据融合前通过imu预积分进行imu数据预处理；融合单元，用于采用eskf算法将补偿后的激光雷达数据与预处理后的imu数据进行融合，并将融合后的车辆位置信息、车辆当前位置的激光雷达点云数据及车道线图像分别与高精地图进行匹配，在匹配成功时完成车辆实时位置的更新。
18.根据本发明说明书的一方面，提供一种无gps信号下二维码、激光雷达与imu融合定位方法，采用所述的系统实现，所述方法包括：构建矿下场景的高精地图；获取路侧端的二维码图像、车道线图像、车辆的激光雷达数据及imu数据；根据获取的二维码图像识别当前二维码所处目标物在高精地图上的绝对位置，并基于所述绝对位置和车辆与目标物之间的相对距离得到车辆在高精地图中的初始位置；在车辆行驶过程中，将车辆的激光雷达数据与imu数据进行融合，并将融合后的imu数据、车辆当前位置的激光雷达点云数据及车道线图像分别与高精地图进行匹配，在匹配成功时进行车辆实时位置的更新。
19.上述技术方案首先构建矿下场景的高精地图，然后依据路侧端二维码图像及激光雷达数据确定车辆初始时在高精地图上的绝对位置，在车辆行驶过程中，则依据imu数据获取车辆的导航参数，依据激光雷达数据对导航参数进行融合修正，并将修正后的导航参数与构建的高精地图进行匹配，若匹配成功，则更新车辆的实时位置。
20.上述技术方案通过车辆的激光雷达数据构建矿下场景的高精地图，并结合二维码数据、激光雷达数据和高精地图确定了车辆的初始位置，解决了现有室内或地下等无gps信号环境下，因卫星导航无法正常使用而导致车辆初始位置难以确定的问题。
21.作为进一步的技术方案，所述方法进一步包括：在车辆启动前，对安装在车辆上的图像采集装置、激光雷达和imu进行联合标定，分别获取图像采集装置与imu之间的外部参数、激光雷达与imu之间的外部参数。
22.作为进一步的技术方案，所述方法进一步包括：在车辆行驶过程中，按照预设周期获取路侧端的二维码图像，并基于获取的二维码图像，重新确定车辆的当前位置。该技术方案在车辆行驶过程中，能够通过重新获取二维码图像来修正因时间累积造成的导航误差，提高车辆定位精度。
23.上述技术方案中的二维码可贴附于路桩上，路桩按照预定距离布设于矿下路径的路侧端，部署方式简单且制作成本低，从而以较低的成本实现了矿下场景车辆初始位置的确定，同时实现了车辆行驶过程中绝对位置的重新确定，达到了低成本、高精度的定位导航效果。
24.作为进一步的技术方案，所述方法进一步包括：利用目标车辆遍历矿下所有路径，并在遍历过程中，获取车辆的激光雷达数据，利用所述激光雷达数据构建矿下场景的高精地图。通过预先构建的高精地图，既便于在车辆启动时确定车辆的初始位置，也便于在车辆行驶过程中确定车辆的实时位置。
25.与现有技术相比，本发明的有益效果在于：（1）本发明提供一种系统，该系统通过构建模块构建矿下场景的高精地图；通过采集模块采集的二维码数据和激光雷达数据结合高精地图确定车辆的初始位置，通过采集模块采集的激光雷达数据、imu数据、车道线图像结合高精地图确定车辆的实时位置，从而实现矿下场景中车辆初始位置及实时位置的准确确定，保证了矿下场景车辆导航定位的精度。
26.（2）本发明采用多种车载传感器结合路侧端二维码的方式实现矿下场景车辆的导航定位，相对于现有无gps场景的定位方法，本发明的制作成本更低、精度更高，能够保证矿下场景车辆的持续、稳定、可靠的高精度定位。
27.（3）本发明提供一种方法，该方法通过车辆的激光雷达数据构建矿下场景的高精地图，并结合二维码数据、激光雷达数据和高精地图确定了车辆的初始位置，解决了现有室内或地下等无gps信号环境下，因卫星导航无法正常使用而导致车辆初始位置难以确定的问题，此外，通过激光雷达数据、imu数据和高精地图确定了车辆的实时位置，解决了矿下场景中的车辆实时位置定位问题。
附图说明
28.图1为根据本发明实施例的无gps信号下二维码、激光雷达与imu融合定位系统的示意图。
29.图2为根据本发明实施例的相机与imu联合标定的示意图。
30.图3为根据本发明实施例的激光雷达与imu联合标定时坐标系的关系图。
31.图4为根据本发明实施例的激光雷达与imu融合定位的示意图。
32.图5为根据本发明实施例的无gps信号下二维码、激光雷达与imu融合定位方法的示意图。
具体实施方式
33.以下将结合附图对本发明各实施例的技术方案进行清楚、完整的描述，显然，所描述发实施例仅仅是本发明的一部分实施例，而不是全部的实施例。基于本发明的实施例，本领域普通技术人员在没有做出创造性劳动的前提下所得到的所有其它实施例，都属于本发明所保护的范围。
34.本发明一方面提供一种无gps信号下二维码、激光雷达与imu融合定位系统，以解决无gps信号环境下存在的卫星导航无法使用、车辆初始位置无法确定、惯性导航无法得到
绝对位置、惯性导航设备误差随时间累积等问题。
35.如图1所示，所述系统包括采集模块、构建模块、车辆初始位置获取模块和车辆实时位置更新模块。所述系统应用于无人车。
36.所述系统还包括联合标定模块，用于根据icp迭代最近点法对激光雷达和imu进行标定以获取激光雷达与imu之间的外部参数，以及根据在线自标定方法对图像采集装置和imu进行标定以获取图像采集装置与imu之间的外部参数。
37.所述系统还包括控制模块，用于在车辆行驶过程中，按照预设周期控制采集模块重新获取路侧端的二维码图像。
38.采集模块包括安装在车辆上的相机、激光雷达和imu。其中，相机用于采集路侧端的二维码图像及车道线图像，激光雷达用于采集车辆的激光雷达数据及车辆与路侧端目标物之间的相对距离，imu用于采集车辆的imu数据。
39.在初始阶段，目标车辆遍历矿下所有路径，采集模块采集车辆的激光雷达数据并输送至构建模块，构建模块基于该激光雷达数据构建矿下场景的高精地图。
40.在车辆启动时，采集模块采集二维码图像、激光雷达数据并输送至车辆初始位置获取模块，车辆初始位置获取模块结合这些数据和高精地图确定车辆的初始位置。
41.在车辆行驶过程中，采集模块采集激光雷达数据、imu数据、车道线图像并输送至车辆实时位置更新模块，车辆实时位置更新模块结合这些数据和高精地图确定车辆的实时位置。
42.作为一种实施方式，联合标定模块中对激光雷达和imu进行标定的过程包括：首先选取某个标志物，由imu惯性传感器和激光雷达采集该标志物的位姿数据，得到该标志物在imu惯导坐标系下的坐标和在激光雷达坐标系下的点云坐标，然后将这两个坐标进行数据关联，得到imu惯导坐标系与激光雷达坐标系的变换矩阵。
43.作为一种实施方式，联合标定模块中对相机与imu进行标定的过程包括：根据某一时刻相机同时具有最大的内点数和最高的阈值幅度来选择一个主相机，以线性方式初始化相机与imu之间的旋转，并使用概率线性滑动窗口处理相机和imu的平移，然后利用高精度的非线性图优化方式去修正标定结果。此外，在标定过程中增加了终止机制来减少计算资源的消耗。
44.构建模块，用于根据车辆遍历矿下所有路径时所获取的激光雷达数据，构建矿下场景的高精地图。具体来说，车辆在矿下将所有路径跑一遍，在该过程中，获取车辆的激光雷达数据，激光雷达从某一点开始利用点云帧间匹配得到变换矩阵，从而得到这一点的位姿，然后输入后端模块去优化一段时间内的车辆的位姿序列，得到准确的位姿，最后利用每一时刻的位姿将点云拼接起来，进而形成高精地图。
45.作为一种实施方式，二维码贴附于路桩上，路桩设置于矿下路径的路侧端。所述路桩沿矿下路径的路侧端间隔布设有多个，且每个路桩上均设有二维码。车辆在行驶过程中，可定时获取路侧端二维码的图像，从而重新确认车辆在高精地图上的绝对位置，修正因时间累积造成的导航误差，进一步提高定位精度。
46.在车辆启动时，相机拍摄路桩上二维码图像并发送至车辆初始位置获取模块，车辆初始位置获取模块根据该二维码图像识别当前路桩在高精地图上的绝对位置，然后再结合激光雷达探测到的车辆与路桩之间的相对距离，得到初始时车辆在高精地图中的位置信
息。
47.在车辆行驶过程中，采集模块将车辆的激光雷达数据与imu数据发送至车辆实时位置更新模块，由车辆实时位置更新模块对激光雷达数据与imu数据进行融合，并将融合后的imu数据、车辆当前位置的激光雷达点云数据及车道线图像分别与高精地图进行匹配，在匹配成功时进行车辆实时位置的更新。
48.所述车辆实时位置更新模块进一步包括：数据补偿单元，用于获取激光雷达点云数据的畸变部分并进行补偿。首先，选取激光雷达某一帧的扫描时间段，根据当前扫描时刻和imu的采样时间段找到最接近时刻的imu惯导数据，利用联合标定中确定的激光雷达与imu之间的外参，采用线性插值的方法求出当前扫描时间段下点云数据相对惯导坐标系下的位姿；然后在原始点云数据中利用该位姿信息对所有的激光点都进行转换，得到点云数据的畸变部分，并将其补偿。
49.预处理单元，用于在激光雷达数据与imu数据融合前通过imu预积分进行imu数据预处理。由于激光雷达和imu惯性传感器的采样频率不同，需要在传感器融合前对imu数据进行预处理，采用的方法是imu预积分。假设imu惯导和激光雷达的时间同步，以激光雷达的采样间隔为基准，相邻两帧点云数据的时刻分别为和，在内对imu惯导数据单独积分，得到的结果就是预积分项。
50.融合单元，用于采用eskf算法将补偿后的激光雷达数据与预处理后的imu数据进行融合，并将融合后的车辆位置信息、车辆当前位置的激光雷达点云数据及车道线图像分别与高精地图进行匹配，在匹配成功时完成车辆实时位置的更新。
51.本发明一方面还提供一种无gps信号下二维码、激光雷达与imu融合定位方法，该方法以高精地图为基础，采用视觉导航、激光导航与惯性导航相结合的方式实现矿下无人车的定位和导航。
52.该方法以预先采集的高精地图为基准，对相机、激光雷达和imu惯性传感器进行联合标定，通过识别路侧端贴放的二维码，结合激光雷达的探测距离，得到车辆在高精地图上的初始位置；然后在运动过程中，以惯性导航为主，激光雷达为辅，利用激光雷达探测到的位置数据去修正惯性导航解算出的导航参数，再结合坐标转换关系，将修正后的导航参数进行坐标转换后与高精地图进行匹配，同时将车辆当前位置的激光雷达点云数据及车道线图像与高精地图进行匹配，且在导航参数、激光雷达点云数据和车道线图像均匹配成功时，进行车辆实时位置的更新。此外，车辆会定时扫描路侧端二维码，重复上述步骤以保证实时定位的可靠性和精度。
53.该方法具体包括以下步骤：步骤1，在无人车启动前，对安装在车辆上的相机、激光雷达和imu惯性传感器进行联合标定，分别获取相机与imu之间的外部参数、激光雷达与imu之间的外部参数。
54.所述的惯性传感器包括陀螺仪和加速度计，标定的具体过程如下：步骤1.1，采用icp迭代最近点法对激光雷达和imu惯导进行标定。
55.首先选取某个标志物，由imu惯性传感器和激光雷达采集该标志物的位姿数据，得到该标志物在imu惯导坐标系下的坐标和在激光雷达坐标系下的点云坐标，然后将这两个坐标进行数据关联，得到imu惯导坐标系与激光雷达坐标系的变换矩阵。
56.步骤1.2，通过在线自标定的方法来对相机与imu进行标定。
57.根据某个时刻相机同时具有最大的内点数和最高的阈值幅度来选择一个主相机，以线性方式初始化相机与imu之间的旋转，并使用概率线性滑动窗口处理相机和imu的平移，然后利用高精度的非线性图优化方式去修正标定结果。此外，在标定过程中增加了终止机制来减少计算资源的消耗。
58.步骤2，使用相机识别路侧端贴放的二维码，获取路侧端在高精地图上的绝对位置信息，再结合激光雷达探测到的相对距离，得到初始时车辆在高精地图中的位置信息。
59.步骤3，无人车启动后，通过激光雷达和imu融合，利用激光雷达的观测数据去修正惯性导航解算出的导航参数，经过坐标转换后与高精地图进行匹配，同时将车辆当前位置的激光雷达点云数据及车道线图像与高精地图进行匹配，且在导航参数、激光雷达点云数据和车道线图像均匹配成功时，完成定位位置的更新。具体过程如下：步骤3.1，点云数据畸变补偿。
60.首先，选取激光雷达某一帧的扫描时间段，根据当前扫描时刻和imu的采样时间段找到最接近时刻的imu惯导数据，利用步骤1中确定的lidar/imu之间的外参，采用线性插值的方法求出当前扫描时间段下点云数据相对惯导坐标系下的位姿。然后在原始点云数据中利用该位姿信息对所有的激光点都进行转换，得到点云数据的畸变部分，并将其补偿。
61.步骤3.2，imu预积分。
62.由于激光雷达和imu惯性传感器的采样频率不同，需要在传感器融合前对imu数据进行预处理，采用的方法是imu预积分。假设imu惯导和激光雷达的时间同步，以激光雷达的采样间隔为基准，相邻两帧点云数据的时刻分别为和，在内对imu惯导数据单独积分，得到的结果就是预积分项，如公式(1)所示。
63.ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(1)式中，表示旋转矩阵预积分量，表示速度预积分量，表示位置预积分量，表示与的时间差，表示时刻角速度，表示时刻陀螺仪零偏，
表示时刻陀螺仪噪声，表示时刻加速度，表示时刻加速度零偏，表示时刻加速度噪声。
64.步骤3.3，基于eskf融合定位。
65.采用误差状态卡尔曼滤波算法(eskf)，将激光雷达定位信息和imu惯导位姿信息进行融合，分为预测和更新两个部分。
66.(1) 预测将imu惯性传感器量测的加速度和角速度信息进行积分处理得到位置、速度、姿态的预测值，如公式(2)所示：
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(2)式中为位置预测值，为速度预测值，为姿态四元数预测值，为加速度计零偏，为陀螺仪零偏，为陀螺仪噪声，为加速度噪声，为姿态阵，为重力加速度，为时间增量。
67.然后分析上式预测结果与真实状态的误差，将误差状态离散化，可得到误差状态空间方程，如公式(3)所示：
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(3)式中，为预测状态，为误差状态向量，为输入控制向量，为随机噪声向量。
68.将误差状态空间方程线性化，可得到误差状态协方差方程，如公式(4)所示：
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(4)式中，为的雅克比矩阵，为随机噪声向量的雅可比矩阵，为的协方差矩阵。
69.(2) 更新量测方程如公式(5)所示：
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(5)式中，为的非线性函数，为真实状态，为高斯白噪声。
70.由量测部分对预测部分进行校正，如公式(6)所示：
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(6)式中，为卡尔曼增益，为误差状态估计值，为误差状态协方差矩阵，为时刻的状态估计值，为雅可比矩阵。
71.最后进行预测状态更新，如公式(7)所示：
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(7)式中，为位置估计值，为速度估计值，为姿态四元数估计值，为加速度计零偏，为陀螺仪零偏，为误差位置估计值，为误差速度估计值，为误差旋转矢量估计值，为误差加速度计零偏估计值，为误差陀螺仪零偏估计值。
72.步骤4，为进一步减小导航误差，可定时扫描路侧端二维码，重复步骤2和步骤3，通过重新获取车辆的当前位置信息来修正因时间累积造成的导航误差，进一步提高定位结果的精度。
73.所述方法还包括，在步骤1之前，利用目标车辆遍历矿下所有路径，并在遍历过程中，获取车辆的激光雷达数据，利用所述激光雷达数据构建矿下场景的高精地图。
74.根据联合估计系统的初始值和外部参数，采用在线标定的方法实现精确的相机-imu外参标定，如图2所示。首先根据某时刻相机同时拥有最大的内点数和最高的阈值幅度来选择一个主相机，以线性方式实现相机和imu之间的旋转标定，然后使用概率线性滑动窗口处理相机和imu之间的平移标定，同时利用高精度的非线性图优化方式去修正标定结果。当标定结果满足终止标准时，停止标定，完成初始化。
75.采用icp迭代最近点法对激光雷达和imu惯导进行联合标定，如图3所示。小车上的三个坐标系分别是：表示地理坐标系，表示imu坐标系，表示激光雷达坐标系。首先采集标志物在地理坐标系下的原始数据和激光雷达的点云数据，根据捷联惯导算法解算出标志物在imu惯导坐标系下的坐标，对点云数据预处理并确定标志物在激光雷达坐标系下的点云坐标，然后将标志物在imu惯导坐标系下的坐标与在激光雷达坐标系下的坐标进行数据关联，得到这两个坐标系的转换矩阵，完成标定。
76.车辆运动过程中利用激光雷达点云数据去修正imu惯导数据的融合过程如图4所示。图4中，采用imu预积分的方法对惯性传感器的数据进行预处理，同时利用线性插值法处理激光雷达点云数据的运动畸变；然后将处理后的imu数据和点云数据作为误差卡尔曼滤
波器的输入，并用滤波后的结果去修正imu数据，实现车辆姿态和位置信息的更新。
77.如图5所示，本发明方法在初始时，车辆自动制动，利用里程计表判别车速是否为0km/h，若是，则进行联合标定。
78.车辆静止后，对相机、激光雷达以及imu惯导进行联合标定。并在标定后，通过相机识别路侧端二维码，结合激光雷达探测到的相对距离和高精地图获取车辆的初始位置。
79.车辆启动后，对imu惯导数据和激光雷达点云数据进行预处理，利用误差状态卡尔曼滤波算法来修正车辆的位置、姿态及速度信息。
80.然后基于厘米级的高精地图分别进行激光雷达点云匹配、车道线识别与地图数据匹配、导航参数与地图数据匹配，判别当前定位信息是否与高精地图匹配成功，若匹配成功，则完成定位更新；否则，重新获取融合数据再进行匹配。
81.本发明通过车辆扫描路侧端二维码来获取初始定位，很好的解决了无gps信号环境下无人车初始位置无法确定的问题，同时在运动过程中利用激光雷达、相机等多传感器数据融合对惯性导航累积误差进行修正，提高了定位系统的可靠性、稳定性和精度，能够在无gps信号等复杂环境下实现连续的自主导航定位，可广泛地应用于矿下以及gps信号弱的室内、地下等多种场景。
82.在本说明书的描述中，参考术语“一个实施方式”、“某些实施方式”、“示意性实施方式”、“示例”、“具体示例”、或“一些示例”等的描述意指结合所述实施方式或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施方式或示例中。在本说明书中，对上述术语的示意性表述不一定指的是相同的实施方式或示例。而且，描述的具体特征、结构、材料或者特点可以在任何的一个或多个实施方式或示例中以合适的方式结合。
83.最后应说明的是：以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述实施例所记载的技术方案进行修改，或者对其中部分或者全部技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明实施例技术方案。