掘进机的位姿标定系统的制作方法

1.本实用新型涉及掘进机位姿测量技术领域，具体而言，涉及一种掘进机的位姿标定系统。


背景技术：

2.相关技术中，标定摄像头坐标系与捷联惯导坐标系的常用方法是通过光学传递方法进行的，具体地，通过在摄像头壳体上安装一个标准的六面体，并通过专业设备调整六面体，使六面体各面分别垂直于摄像头坐标系的各轴，此时摄像头壳体上六面体各面的法线即可代表摄像头坐标系各轴向；另一方面，在捷联惯导壳体上也安装一个标准的六面体，通过专业设备调整六面体，使六面体各面分别垂直于惯导坐标系的各轴，此时惯导壳体上六面体各面的法线即可代表惯导坐标系各轴向。完成上述工作后，将惯导安装到用户系统中，通过经纬仪即可测到两个六面体之间的角度关系，即完成了安装角标定。
3.上述方案非常直观，但存在以下问题：摄像头壳体上的六面体必须在专业的光学实验室提前安装并校准，操作复杂，试验费用也很高。惯导壳体上的六面体需要惯导厂家在专业的惯性技术实验室提前安装并校准，操作复杂，专业性要求较高，试验费用也很高；另外，六面体的成本也较高，一般用于要求极高的军用高精度领域。
4.针对上述相关技术中存在的技术问题，目前没有提出有效的解决方案。


技术实现要素：

5.鉴于上述问题，本实用新型提出了一种掘进机的位姿标定系统，以至少解决相关技术中掘进机的位姿标定操作复杂、成本高的技术问题。所述技术方案如下：
6.本实用新型提供了一种掘进机的位姿标定系统，所述系统至少包括掘进机、捷联惯导、第一图像采集装置、第二图像采集装置、激光靶以及激光指向仪，其中，所述激光靶和所述激光指向仪均设于巷道内，所述激光靶悬挂在所述激光指向仪前侧，所述激光指向仪用于在所述激光靶面上形成光斑；所述第一图像采集装置和所述第二图像采集装置均设于所述掘进机上，用于通过采集所述激光靶上的光斑图像；所述捷联惯导，设于所述掘进机上，用于确定所述图像采集装置坐标系相对所述捷联惯导的安装矩阵，其中，所述安装矩阵用于表征所述图像采集装置坐标系相对所述捷联惯导的安装角，以标定所述捷联惯导在所述巷道内的位姿信息。
7.在一种可能的实现方式中，所述系统还包括激光测距装置，其中，所述激光测距装置，设于所述巷道内，且设于所述第一图像采集装置、所述第二图像采集装置和所述激光靶之间，用于测量所述第一图像采集装置、所述第二图像采集装置和所述激光靶面上的光斑相对所述激光测距装置的位移矢量。
8.在另一种可能的实现方式中，所述激光测距装置为全站仪。
9.在另一种可能的实现方式中，所述激光指向仪发射的光线平行于所述巷道设定的水平面方向。
10.在另一种可能的实现方式中，所述第一图像采集装置为第一摄像头，所述第二图像采集装置为第二摄像头。
11.在另一种可能的实现方式中，所述第一图像采集装置相对所述光斑的连线方向与所述第二图像采集装置相对所述光斑的连线方向空间内不平行。
12.在另一种可能的实现方式中，所述激光靶安装于所述掘进机后方巷道顶面或侧面上。
13.在另一种可能的实现方式中，所述激光靶采用半透明材料制作而成。
14.借由上述技术方案，本实用新型实施例提供的掘进机的位姿标定系统，通过在掘进机上设两个图像采集装置，在巷道内设激光靶和激光指向仪，通过激光指向仪在激光靶面上形成光斑，捷联惯导设于所述掘进机上，掘进机位于巷道内；首先，获取两个图像采集装置相对激光靶面上光斑的两个位移矢量，且两个位移矢量空间内不平行；然后，根据两个位移矢量以及图像采集装置坐标系相对捷联惯导壳体坐标系的投影关系确定图像采集装置坐标系相对捷联惯导的安装矩阵，即得到图像采集装置坐标系相对捷联惯导的安装角，以实现标定捷联惯导在巷道内的位姿信息，操作简单，成本低，解决了相关技术中掘进机的位姿标定操作复杂、成本高的技术问题，进而提高了捷联惯导在巷道内位姿信息的准确度。
附图说明
15.为了更清楚地说明本实用新型实施例的技术方案，下面将对本实用新型实施例描述中所需要使用的附图作简单地介绍。
16.图1是根据本实用新型实施例提供的一种基于双目视觉和捷联惯导的掘进机位姿测量系统示意图；
17.图2是根据本实用新型实施提供的一种掘进机的位姿标定涉及的坐标系标定的示意图；
18.图3是根据本实用新型实施例提供的安装角标定方法原理示意图。
具体实施方式
19.下面将参照附图更详细地描述本实用新型的示例性实施例。虽然附图中显示了本实用新型的示例性实施例，然而应当理解，可以以各种形式实现本实用新型而不应被这里阐述的实施例所限制。相反，提供这些实施例是为了能够更透彻地理解本实用新型，并且能够将本实用新型的范围完整的传达给本领域的技术人员。
20.需要说明的是，本实用新型的说明书和权利要求书及上述附图中的术语“第一”、“第二”等是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。应该理解这样使用在适当情况下可以互换，以便这里描述的本实用新型的实施例能够以除了在这里图示或描述的那些以外的顺序实施。此外，术语“包括”及其变体要被解读为意味着“包括但不限于”的开放式术语。
21.为了解决相关技术存在的技术问题，在本实施例中提供了一种掘进机的位姿标定系统。下面以具体地实施例对本实用新型的技术方案以及本实用新型的技术方案如何解决上述技术问题进行详细说明。下面这几个具体的实施例可以相互结合，对于相同或相似的概念或过程可能在某些实施例中不再赘述。下面将结合附图，对本实用新型的实施例进行
描述。
22.在本案的一个应用场景中，采用掘进机挖掘巷道时，要求挖掘出的巷道是沿着激光指向仪给出的方向，且上下左右边界相对激光指向仪光线的距离满足预定要求，通过在车上安装两个摄像头，采用半透明材料制作激光靶并将其悬挂在激光指向仪前面，以保证激光指向仪能够照到激光靶面形成光斑，然后通过两个摄像头同时采集光斑图像，基于双目视觉技术即可确定车辆相对光斑的位移。双目视觉确定的车辆相对于光斑的位移是在相机坐标系中表示的，必须将其转换到巷道坐标系才能与光斑坐标相加得到车辆在巷道坐标系中的坐标，如果已知摄像头坐标系和提供航姿信息的捷联惯导之间的安装角度时，可通过航姿信息将其变换到巷道坐标系，从而确定车身在巷道坐标系中的位置信息。如何准确标定摄像头坐标系相对捷联惯导的安装角是上述方案成功与否法关键。
23.本实用新型提供一种掘进机的位姿标定系统，该系统至少包括掘进机、捷联惯导、第一图像采集装置、第二图像采集装置、激光靶以及激光指向仪，其中，
24.激光靶和激光指向仪均设于巷道内，激光靶悬挂在激光指向仪前侧，激光指向仪用于在激光靶面上形成光斑；
25.其中，激光指向仪发射的光线平行于巷道设定的水平面方向。
26.第一图像采集装置和第二图像采集装置均设于掘进机上，用于通过采集激光靶上的光斑图像；
27.在本实施例中，通过光斑图像获取第一图像采集装置相对光斑的第一位移矢量，以及第二图像采集装置相对光斑的第二位移矢量；其中，第一位移矢量和第二位移矢量空间内不平行；
28.可选的，本实施例中的图像采集装置为摄像头。
29.在本实施例中，利用双目视觉标定摄像头相对捷联惯导的安装角，是激光指向仪位置信息引入的关键。其中，双目检测的原理：通过对两幅图像视差的计算，直接对前方景物(图像所拍摄到的范围)进行距离测量，而无需判断前方出现的是什么类型的障碍物。所以对于任何类型的障碍物，都能根据距离信息的变化，进行必要的预警或制动；双目摄像头的原理与人眼相似，人眼能够感知物体的远近，是由于两只眼睛对同一个物体呈现的图像存在差异，也称“视差”。物体距离越远，视差越小；反之，视差越大。双目视觉原理一般由双摄像机(即上述图像采集装置)从不同角度同时获得被测物的两幅数字图像；并基于视差原理恢复出物体的三维几何信息，重建物体三维轮廓及位置。
30.在本实施例中，利用第一位移矢量和第二位移矢量空间内不平行的特性，来完成图像采集装置坐标系相对捷联惯导的安装矩阵的标定。通过本实施例实现激光指向仪位置信息引入，代替了人工对点，不仅减少了人力还提高了准确度。
31.捷联惯导，设于掘进机上，用于确定图像采集装置坐标系相对捷联惯导的安装矩阵，其中，安装矩阵用于表征图像采集装置坐标系相对捷联惯导的安装角，以标定捷联惯导在巷道内的位姿信息。
32.可选的，通过根据图像采集装置坐标系相对捷联惯导壳体坐标系的第一投影关系、第一位移矢量和第二位移矢量确定安装矩阵。
33.在本案的一个可选的实施例中，该系统还包括激光测距装置，其中，激光测距装置，设于巷道内，且设于第一图像采集装置、第二图像采集装置和激光靶之间，用于测量第
一图像采集装置、第二图像采集装置和激光靶面上的光斑相对激光测距装置的位移矢量。
34.在本案的一个可选的实施例中，激光测距装置为全站仪。本实施例中的激光测距装置优先全站仪。
35.为了标定安装矩阵，采用全站仪对和进行间接测量，通过将全站仪置于摄像头3和4与激光靶面之间，采用全站仪即可实现摄像头相对捷联惯导安装角的标定，操作简单，便于使用现场的操作。
36.在本案的一个可选的实施例中，第一图像采集装置为第一摄像头，第二图像采集装置为第二摄像头。
37.在本案的一个可选的实施例中，第一图像采集装置相对光斑的连线方向与第二图像采集装置相对光斑的连线方向空间内不平行。
38.在本案的一个可选的实施例中，激光靶安装于掘进机后方巷道顶面或侧面上。
39.在本案的一个可选的实施例中，激光靶采用半透明材料制作而成。
40.本实用新型实施例提供的掘进机的位姿标定系统，通过在掘进机上设两个图像采集装置，在巷道内设激光靶和激光指向仪，通过激光指向仪在激光靶面上形成光斑，捷联惯导设于掘进机上，掘进机位于巷道内；首先，获取两个图像采集装置相对激光靶面上光斑的两个位移矢量，且两个位移矢量空间内不平行；然后，根据两个位移矢量以及图像采集装置坐标系相对捷联惯导壳体坐标系的投影关系确定图像采集装置坐标系相对捷联惯导的安装矩阵，即得到图像采集装置坐标系相对捷联惯导的安装角，实现了标定捷联惯导在巷道内的位姿信息，操作简单，成本低，解决了相关技术中掘进机的位姿标定操作复杂、成本高的技术问题，进而提高了捷联惯导在巷道内位姿信息的准确度。
41.在本案的一个实施例中，针对的系统如图1所示，图1是根据本实用新型实施例提供的一种基于双目视觉和捷联惯导的掘进机位姿测量系统示意图，该系统包括安装在掘进机车身1上的捷联惯导2，安装在掘进机车身1上的摄像头3(即上述第一图像采集装置)和摄像头4(即上述第二图像采集装置)、安装在巷道上的激光靶5、安装在巷道上的激光指向仪6，以及在激光靶面上形成的光斑7。
42.图2是根据本实用新型实施提供的一种掘进机的位姿标定涉及的坐标系标定的示意图，针对图1提供的系统，对系统中各个设备进行坐标系标定。
43.其中，巷道坐标系t的原点位于激光指向仪上，y
t
在水平面内沿激光发射方向，z
t
轴竖直向上，x
t
、y
t
和z
t
构成右手直角坐标系。摄像头4的坐标系c(即上述图像采集装置坐标系)，简称摄像头坐标系，其原点位于摄像头光心，zc沿摄像头光轴方向，xc在焦平面内向右且与zc垂直，xc、yc和zc构成右手直角坐标系。捷联惯导壳体坐标系b固连于捷联惯导壳体，xb轴指向车身右侧，yb轴指向车身前方，zb轴指向车身上方，xb、yb和zb构成右手直角坐标系。地理坐标系n在图3中没有画出，该坐标系原点与b系原点重合，xn指向正东，yn指向正北，zn指向天，xn、yn和zn构成右手直角坐标系。
44.掘进机需要捷联惯导提供其相对巷道坐标系的位移信息(即上述位姿信息)，如公式(1)：
45.46.其中：为导航系巷道坐标系n到巷道坐标系t的旋转矩阵，是巷道设计值，属于已知量；有捷联惯导给出，属于已知量；为捷联惯导相对摄像头4的位移矢量在惯导壳体系b的投影，属于已知量；为摄像头4相对激光靶面上光斑的位移矢量r4在摄像头坐标系的投影，由摄像头3和摄像头4构成的双目视觉系统测量；为摄像头坐标系相对捷联惯导的安装矩阵，必须在系统投入使用前通过标定方法确定。
47.摄像头3相对激光靶面上光斑的位移矢量r3(第一位移矢量)和摄像头4相对激光靶面上光斑的位移矢量r4(第二位移矢量)不平行，本实用新型根据这一特性进行摄像头坐标系c相对捷联惯导壳体系b的安装矩阵的标定。
48.摄像头3和摄像头4根据双目视觉原理测得矢量r3和r4在摄像头4的坐标系c(图像采集装置坐标系)内的投影和根据r3和r4在c和b系的投影关系得公式(2)。
[0049][0050]
在计算得到和之后，可计算得到上述公式(3)。
[0051][0052]
为标定安装矩阵采用全站仪对和进行间接测量，测量原理见图3，图3是根据本实用新型实施例提供的安装角标定方法原理示意图。
[0053]
图3中o为全站仪所在位置，全站仪输出结果为和即图3中ra、rb和rc在巷道坐标系的投影。根据矢量合成法则得公式(4)和公式(5)：
[0054][0055][0056]
进一步利用和两个已知量将和变换到b系为公式(6)和公式(7)：
[0057][0058][0059]
将和带入公式(2)即可得到的标定结果。
[0060]
考虑到和都存在测量误差，所以实际标定算法首先将上述测量结果归一化：
[0061][0062]
对各个投影矢量的矩阵进行正交化，得到正交矩阵，来计算安装角，可以减小测量误差对角度标定结果的影响。
[0063]
然后构造与v3和v4正交的归一化矢量和
[0064][0065]
则和存在以下关系：
[0066][0067]
由此得：
[0068][0069]
综上，本实用新型工作过程包括如下步骤:
[0070]
步骤1:根据双目视觉原理测得摄像头3相对激光靶面上光斑的位移矢量r3在摄像头4的坐标系c内的投影见附图2；
[0071]
步骤2:根据双目视觉原理测得摄像头4相对激光靶面上光斑的位移矢量r4在摄像头4的坐标系c内的投影见附图2；
[0072]
步骤3:将全站仪置于摄像头3和4与激光靶面之间，测量摄像头3、摄像头4和激光靶面上的光斑相对于全站仪的矢量和
[0073]
步骤4:计算r3和r4在巷道坐标系的投影和
[0074][0075][0076]
步骤5:计算r3和r4在惯导壳体坐标系b的投影和
[0077][0078][0079]
步骤6:计算和的归一化矢量，得到单位正交矩阵：
[0080][0081]
步骤7:构造与v3和v4正交的归一化矢量和
[0082][0083]
步骤8:计算最终的标定结果
[0084][0085]
通过本实用新型实施例至少具有以下有益效果：
[0086]
(1)实现激光指向仪位置信息引入，替代人工对点；
[0087]
(2)标定双目视觉摄像头相对捷联惯导的安装角，是激光指向仪位置信息引入的关键；
[0088]
(3)采用全站仪即可实现摄像头相对捷联惯导安装角的标定，操作简单，便于使用现场的操作；
[0089]
(4)本实用新型不需要陀螺寻北仪和平行光管等专业设备，费用低。
[0090]
显然，本领域的技术人员应该明白，上述的本实用新型的各模块或各步骤可以用通用的计算装置来实现，它们可以集中在单个的计算装置上，或者分布在多个计算装置所组成的网络上，可选地，它们可以用计算装置可执行的程序代码来实现，从而，可以将它们存储在存储装置中由计算装置来执行，并且在某些情况下，可以以不同于此处的顺序执行所示出或描述的步骤，或者将它们分别制作成各个集成电路模块，或者将它们中的多个模块或步骤制作成单个集成电路模块来实现。这样，本实用新型不限制于任何特定的硬件和软件结合。
[0091]
以上所述仅为本实用新型的优选实施例而已，并不用于限制本实用新型，对于本领域的技术人员来说，本实用新型可以有各种更改和变化。凡在本实用新型的原则之内，所作的任何修改、等同替换、改进等，均应包含在本实用新型的保护范围之内。