CN112665613A - 掘进机的位姿标定方法及系统 - Google Patents
掘进机的位姿标定方法及系统 Download PDFInfo
- Publication number
- CN112665613A CN112665613A CN202011531231.7A CN202011531231A CN112665613A CN 112665613 A CN112665613 A CN 112665613A CN 202011531231 A CN202011531231 A CN 202011531231A CN 112665613 A CN112665613 A CN 112665613A
- Authority
- CN
- China
- Prior art keywords
- vector
- coordinate system
- projection
- image acquisition
- acquisition device
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 33
- 239000013598 vector Substances 0.000 claims abstract description 273
- 238000006073 displacement reaction Methods 0.000 claims abstract description 118
- 238000009434 installation Methods 0.000 claims abstract description 49
- 239000011159 matrix material Substances 0.000 claims abstract description 38
- 238000010606 normalization Methods 0.000 claims description 16
- 238000012545 processing Methods 0.000 claims description 10
- 230000003287 optical effect Effects 0.000 claims description 7
- 230000015572 biosynthetic process Effects 0.000 claims description 4
- 238000003786 synthesis reaction Methods 0.000 claims description 4
- 238000005259 measurement Methods 0.000 abstract description 9
- 238000010586 diagram Methods 0.000 description 5
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 230000008569 process Effects 0.000 description 2
- 238000012360 testing method Methods 0.000 description 2
- 238000009412 basement excavation Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 239000000463 material Substances 0.000 description 1
- 238000012546 transfer Methods 0.000 description 1
Images
Landscapes
- Navigation (AREA)
Abstract
本发明涉及掘进机位姿测量技术领域，提供了一种掘进机的位姿标定方法及系统，该方法包括：获取第一图像采集装置相对激光靶面上光斑的第一位移矢量以及第二图像采集装置相对光斑的第二位移矢量，其中，第一位移矢量和第二位移矢量空间内不平行，激光靶悬挂在激光指向仪前侧，激光指向仪用于在激光靶面上形成光斑；根据第一位移矢量、第二位移矢量以及图像采集装置坐标系相对捷联惯导壳体坐标系的第一投影关系，确定图像采集装置坐标系相对捷联惯导的安装矩阵；通过安装矩阵确定图像采集装置坐标系相对捷联惯导的安装角，以标定捷联惯导在巷道内的位姿信息。通过本发明，解决了相关技术中掘进机的位姿标定操作复杂、成本高的技术问题。
Description
技术领域
本发明涉及掘进机位姿测量技术领域，具体而言，涉及一种掘进机的位姿标定方法及系统。
背景技术
相关技术中，标定摄像头坐标系与捷联惯导坐标系的常用方法是通过光学传递方法进行的，具体地，通过在摄像头壳体上安装一个标准的六面体，并通过专业设备调整六面体，使六面体各面分别垂直于摄像头坐标系的各轴，此时摄像头壳体上六面体各面的法线即可代表摄像头坐标系各轴向；另一方面，在捷联惯导壳体上也安装一个标准的六面体，通过专业设备调整六面体，使六面体各面分别垂直于惯导坐标系的各轴，此时惯导壳体上六面体各面的法线即可代表惯导坐标系各轴向。完成上述工作后，将惯导安装到用户系统中，通过经纬仪即可测到两个六面体之间的角度关系，即完成了安装角标定。
上述方案非常直观，但存在以下问题：摄像头壳体上的六面体必须在专业的光学实验室提前安装并校准，操作复杂，试验费用也很高。惯导壳体上的六面体需要惯导厂家在专业的惯性技术实验室提前安装并校准，操作复杂，专业性要求较高，试验费用也很高；另外，六面体的成本也较高，一般用于要求极高的军用高精度领域。
针对上述相关技术中存在的技术问题，目前没有提出有效的解决方案。
发明内容
鉴于上述问题，本发明提出了一种掘进机的位姿标定方法及系统，以至少解决相关技术中掘进机的位姿标定操作复杂、成本高的技术问题。所述技术方案如下：
第一方面，提供了一种掘进机的位姿标定方法，包括：获取第一图像采集装置相对激光靶面上光斑的第一位移矢量，以及第二图像采集装置相对所述光斑的第二位移矢量，其中，所述第一位移矢量和所述第二位移矢量空间内不平行，所述激光靶悬挂在激光指向仪前侧，所述激光指向仪用于在所述激光靶面上形成所述光斑；根据所述第一位移矢量、所述第二位移矢量以及图像采集装置坐标系相对捷联惯导壳体坐标系的第一投影关系，确定所述图像采集装置坐标系相对捷联惯导的安装矩阵，其中，所述捷联惯导设于所述掘进机上；通过所述安装矩阵确定所述图像采集装置坐标系相对所述捷联惯导的安装角，以标定所述捷联惯导在巷道内的位姿信息。
在一种可能的实现方式中，根据所述第一位移矢量、所述第二位移矢量以及图像采集装置坐标系相对捷联惯导壳体坐标系的第一投影关系，确定所述图像采集装置坐标系相对捷联惯导的安装矩阵，包括：基于双目视觉算法确定所述第一位移矢量在所述图像采集装置坐标系内的第一投影矢量，以及所述第二位移矢量在所述图像采集装置坐标系内的第二投影矢量，其中，所述图像采集装置坐标系是基于所述第一图像采集装置或第二图像采集装置的中心位置为原点、光轴方向为z轴、焦平面且与z垂直方向为x轴构建的空间直角坐标系；确定所述第一位移矢量在所述捷联惯导壳体坐标系内的第三投影矢量，以及所述第二位移矢量在所述捷联惯导壳体坐标系内的第四投影矢量，其中，所述捷联惯导壳体坐标系是以所述捷联惯导的壳体为原点、掘进机的前进方向为y轴、掘进机竖直面且与y轴竖直方向为z轴构建的空间直角坐标系；根据所述第一投影矢量、所述第二投影矢量、所述第三投影矢量、所述第四投影矢量和所述第一投影关系，计算所述安装矩阵。
在另一种可能的实现方式中，确定所述第一位移矢量在所述捷联惯导壳体坐标系内的第三投影矢量，以及所述第二位移矢量在所述捷联惯导壳体坐标系内的第四投影矢量，包括：获取所述第一图像采集装置相对激光测距装置的第三位移矢量；以及，获取所述第二图像采集装置相对所述激光测距装置的第四位移矢量；以及，获取所述激光测距装置相对所述光斑的第五位移矢量，其中，所述激光测距装置设于所述巷道内，位于所述第一图像采集装置、所述第二图像采集装置和所述激光靶之间；分别测量所述第三位移矢量、所述第四位移矢量和所述第五位移矢量在巷道坐标系内对应的第五投影矢量、第六投影矢量和第七投影矢量；根据所述第五投影矢量、所述第六投影矢量、所述第七投影矢量、地理坐标系相对所述巷道坐标系的第二投影关系、所述捷联惯导壳体坐标系相对所述地理坐标系的第三投影关系，计算所述第三投影矢量和所述第四投影矢量，其中，所述巷道坐标系是以所述激光指向仪为原点、水平面内激光发射方向为y轴、竖直方向为z轴构建的空间直角坐标系。
在另一种可能的实现方式中，根据所述第五投影矢量、所述第六投影矢量、所述第七投影矢量、地理坐标系相对所述巷道坐标系的第二投影关系、所述捷联惯导壳体坐标系相对地理坐标系的第三投影关系，计算所述第三投影矢量和所述第四投影矢量，包括：基于矢量合成法则，根据所述第五投影矢量、所述第六投影矢量和所述第七投影矢量计算所述第一图像采集装置在巷道坐标系内的第八投影矢量，以及，所述第二图像采集装置在巷道坐标系内的第九投影矢量；根据所述第二投影关系和所述第三投影关系将所述第八投影矢量和所述第九投影矢量转换至捷联惯导壳体坐标系，得到所述第三投影矢量和所述第四投影矢量，其中，所述第二投影关系和所述第三投影关系为已知量。
在另一种可能的实现方式中，在根据所述第一投影矢量、所述第二投影矢量、所述第三投影矢量、所述第四投影矢量和所述第一投影关系计算所述安装矩阵之后，所述方法还包括：分别对所述第一投影矢量、所述第二投影矢量、所述第三投影矢量和所述第四投影矢量进行归一化处理；根据归一化处理结果构造所述第一图像采集装置的第一归一化矢量和所述第二图像采集装置的第二归一化矢量，其中，所述归一化处理结果均为单位正交矩阵；根据所述第一归一化矢量和所述第二归一化矢量标定所述安装矩阵。
第二方面，还提供了一种掘进机的位姿标定系统，所述系统至少包括掘进机、捷联惯导、第一图像采集装置、第二图像采集装置、激光靶以及激光指向仪，其中，所述激光靶和所述激光指向仪均设于巷道内，所述激光靶悬挂在所述激光指向仪前侧，所述激光指向仪用于在所述激光靶面上形成光斑；所述第一图像采集装置和所述第二图像采集装置均设于所述掘进机上，用于通过采集所述激光靶上的光斑图像获取所述第一图像采集装置相对所述光斑的第一位移矢量以及所述第二图像采集装置相对所述光斑的第二位移矢量，其中，所述第一位移矢量和所述第二位移矢量空间内不平行；所述捷联惯导，设于所述掘进机上，用于根据图像采集装置坐标系相对捷联惯导壳体坐标系的第一投影关系、所述第一位移矢量和所述第二位移矢量确定所述图像采集装置坐标系相对所述捷联惯导的安装矩阵，其中，所述安装矩阵用于表征所述图像采集装置坐标系相对所述捷联惯导的安装角，以标定所述捷联惯导在所述巷道内的位姿信息。
在一种可能的实现方式中，所述系统还包括激光测距装置，其中，所述激光测距装置，设于所述巷道内，且设于所述第一图像采集装置、所述第二图像采集装置和所述激光靶之间，用于测量所述第一图像采集装置、所述第二图像采集装置和所述激光靶面上的光斑相对所述激光测距装置的位移矢量。
在另一种可能的实现方式中，所述激光测距装置为全站仪。
在另一种可能的实现方式中，所述激光指向仪发射的光线平行于所述巷道设定的水平面方向。
在另一种可能的实现方式中，所述激光靶安装于所述掘进机后方巷道顶面或侧面上。
借由上述技术方案，本发明实施例提供的掘进机的位姿标定方法，通过在掘进机上设两个图像采集装置，在巷道内设激光靶和激光指向仪，通过激光指向仪在激光靶面上形成光斑，捷联惯导设于所述掘进机上，掘进机位于巷道内；首先，获取两个图像采集装置相对激光靶面上光斑的两个位移矢量，且两个位移矢量空间内不平行；然后，根据两个位移矢量以及图像采集装置坐标系相对捷联惯导壳体坐标系的投影关系确定图像采集装置坐标系相对捷联惯导的安装矩阵，即得到图像采集装置坐标系相对捷联惯导的安装角，实现了标定捷联惯导在巷道内的位姿信息，操作简单，成本低，解决了相关技术中掘进机的位姿标定操作复杂、成本高的技术问题，进而提高了捷联惯导在巷道内位姿信息的准确度。
附图说明
为了更清楚地说明本发明实施例的技术方案，下面将对本发明实施例描述中所需要使用的附图作简单地介绍。
图1是根据本发明实施例的一种掘进机的标定方法的流程图；
图2是根据本发明实施例提供的一种基于双目视觉和捷联惯导的掘进机位姿测量系统示意图；
图3是根据本发明实施提供的一种掘进机的位姿标定涉及的坐标系标定的示意图；
图4是根据本发明实施例提供的安装角标定方法原理示意图。
具体实施方式
下面将参照附图更详细地描述本发明的示例性实施例。虽然附图中显示了本发明的示例性实施例，然而应当理解，可以以各种形式实现本发明而不应被这里阐述的实施例所限制。相反，提供这些实施例是为了能够更透彻地理解本发明，并且能够将本发明的范围完整的传达给本领域的技术人员。
需要说明的是，本发明的说明书和权利要求书及上述附图中的术语“第一”、“第二”等是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。应该理解这样使用在适当情况下可以互换，以便这里描述的本发明的实施例能够以除了在这里图示或描述的那些以外的顺序实施。此外，术语“包括”及其变体要被解读为意味着“包括但不限于”的开放式术语。
为了解决相关技术存在的技术问题，在本实施例中提供了一种掘进机的位姿标定方法。下面以具体地实施例对本发明的技术方案以及本发明的技术方案如何解决上述技术问题进行详细说明。下面这几个具体的实施例可以相互结合，对于相同或相似的概念或过程可能在某些实施例中不再赘述。下面将结合附图，对本发明的实施例进行描述。
在本案的一个应用场景中，采用掘进机挖掘巷道时，要求挖掘出的巷道是沿着激光指向仪给出的方向，且上下左右边界相对激光指向仪光线的距离满足预定要求，通过在车上安装两个摄像头，采用半透明材料制作激光靶并将其悬挂在激光指向仪前面，以保证激光指向仪能够照到激光靶面形成光斑，然后通过两个摄像头同时采集光斑图像，基于双目视觉技术即可确定车辆相对光斑的位移。双目视觉确定的车辆相对于光斑的位移是在相机坐标系中表示的，必须将其转换到巷道坐标系才能与光斑坐标相加得到车辆在巷道坐标系中的坐标，如果已知摄像头坐标系和提供航姿信息的捷联惯导之间的安装角度时，可通过航姿信息将其变换到巷道坐标系，从而确定车身在巷道坐标系中的位置信息。如何准确标定摄像头坐标系相对捷联惯导的安装角是上述方案成功与否法关键。
本发明提供一种掘进机的位姿标定方法，图1是根据本发明实施例的一种掘进机的标定方法的流程图，如图1所示，该流程包括如下步骤：
步骤S102，获取第一图像采集装置相对激光靶面上光斑的第一位移矢量，以及第二图像采集装置相对光斑的第二位移矢量；
在本实施例中，第一图像采集装置和第二图像采集装置均设于掘进上，第一位移矢量和第二位移矢量空间内不平行，激光靶和激光指向仪均设于巷道内，激光靶悬挂在激光指向仪前侧，激光指向仪用于在激光靶面上形成光斑；可选的，激光靶安装于掘进机后方巷道顶面或侧面上。
可选的，本实施例中的图像采集装置为摄像头。
在本实施例中，利用第一位移矢量和第二位移矢量空间内不平行的特性，来完成图像采集装置坐标系相对捷联惯导的安装矩阵的标定。通过本实施例实现激光指向仪位置信息引入，代替了人工对点，不仅减少了人力还提高了准确度。
步骤S104，根据第一位移矢量、第二位移矢量以及图像采集装置坐标系相对捷联惯导壳体坐标系的第一投影关系，确定图像采集装置坐标系相对捷联惯导的安装矩阵；
其中，捷联惯导设于掘进机上。
步骤S106，通过安装矩阵确定图像采集装置坐标系相对捷联惯导的安装角，以标定捷联惯导在巷道内的位姿信息。
本发明实施例提供的掘进机的位姿标定方法，通过在掘进机上设两个图像采集装置，在巷道内设激光靶和激光指向仪，通过激光指向仪在激光靶面上形成光斑，捷联惯导设于掘进机上，掘进机位于巷道内；首先，获取两个图像采集装置相对激光靶面上光斑的两个位移矢量，且两个位移矢量空间内不平行；然后，根据两个位移矢量以及图像采集装置坐标系相对捷联惯导壳体坐标系的投影关系确定图像采集装置坐标系相对捷联惯导的安装矩阵，即得到图像采集装置坐标系相对捷联惯导的安装角，实现了标定捷联惯导在巷道内的位姿信息，操作简单，成本低，解决了相关技术中掘进机的位姿标定操作复杂、成本高的技术问题，进而提高了捷联惯导在巷道内位姿信息的准确度。
在一种可能的实现方式中，根据第一位移矢量、第二位移矢量以及图像采集装置坐标系相对捷联惯导壳体坐标系的第一投影关系，确定图像采集装置坐标系相对捷联惯导的安装矩阵，包括：基于双目视觉算法确定第一位移矢量在图像采集装置坐标系内的第一投影矢量，以及第二位移矢量在图像采集装置坐标系内的第二投影矢量，其中，图像采集装置坐标系是基于第一图像采集装置或第二图像采集装置的中心位置为原点、光轴方向为z轴、焦平面且与z垂直方向为x轴构建的空间直角坐标系；确定第一位移矢量在捷联惯导壳体坐标系内的第三投影矢量，以及第二位移矢量在捷联惯导壳体坐标系内的第四投影矢量，其中，捷联惯导壳体坐标系是以捷联惯导的壳体为原点、掘进机的前进方向为y轴、掘进机竖直面且与y轴竖直方向为z轴构建的空间直角坐标系；根据第一投影矢量、第二投影矢量、第三投影矢量、第四投影矢量和第一投影关系，计算安装矩阵。
在本实施例中，利用双目视觉标定摄像头相对捷联惯导的安装角，是激光指向仪位置信息引入的关键。其中，双目检测的原理：通过对两幅图像视差的计算，直接对前方景物(图像所拍摄到的范围)进行距离测量，而无需判断前方出现的是什么类型的障碍物。所以对于任何类型的障碍物，都能根据距离信息的变化，进行必要的预警或制动；双目摄像头的原理与人眼相似，人眼能够感知物体的远近，是由于两只眼睛对同一个物体呈现的图像存在差异，也称“视差”。物体距离越远，视差越小；反之，视差越大。双目视觉原理一般由双摄像机(即上述图像采集装置)从不同角度同时获得被测物的两幅数字图像；并基于视差原理恢复出物体的三维几何信息，重建物体三维轮廓及位置。
在本案的一个实施例中，针对的系统如图2所示，图2是根据本发明实施例提供的一种基于双目视觉和捷联惯导的掘进机位姿测量系统示意图，该系统包括安装在掘进机车身1上的捷联惯导2，安装在掘进机车身1上的摄像头3(即上述第一图像采集装置)和摄像头4(即上述第二图像采集装置)、安装在巷道上的激光靶5、安装在巷道上的激光指向仪6，以及在激光靶面上形成的光斑7。
在本实施例中，摄像头3和摄像头4根据双目视觉原理测得矢量r3(即上述第一位移矢量)和r4(即上述第二位移矢量)在摄像头4的坐标系c(即上述图像采集装置坐标系)内的投影
(即上述第一投影矢量)和
(即上述第二投影矢量)。根据r3和r4在c和b系的投影关系(即上述第一投影关系)得公式(1)。
在本案的一个可选的实施例中，确定第一位移矢量在捷联惯导壳体坐标系内的第三投影矢量，以及第二位移矢量在捷联惯导壳体坐标系内的第四投影矢量，包括：获取第一图像采集装置相对激光测距装置的第三位移矢量；以及，获取第二图像采集装置相对激光测距装置的第四位移矢量；以及，获取激光测距装置相对光斑的第五位移矢量，其中，激光测距装置设于巷道内，位于第一图像采集装置、第二图像采集装置和激光靶之间；分别测量第三位移矢量、第四位移矢量和第五位移矢量在巷道坐标系内对应的第五投影矢量、第六投影矢量和第七投影矢量；根据第五投影矢量、第六投影矢量、第七投影矢量、地理坐标系相对巷道坐标系的第二投影关系、捷联惯导壳体坐标系相对地理坐标系的第三投影关系计算第三投影矢量和第四投影矢量，其中，巷道坐标系是以激光指向仪为原点、水平面内激光发射方向为y轴、竖直方向为z轴构建的空间直角坐标系。
在本实施例中，基于上述公式(1)，如果能够测得
(即上述第三投影矢量)和
(即上述第四投影矢量)，可得安装矩阵，即公式(2)。
在本案的一个可选的实施例中，根据第五投影矢量、第六投影矢量、第七投影矢量、地理坐标系相对巷道坐标系的第二投影关系、捷联惯导壳体坐标系相对地理坐标系的第三投影关系，计算第三投影矢量和第四投影矢量，包括：基于矢量合成法则，根据第五投影矢量、第六投影矢量和第七投影矢量，计算第一图像采集装置在巷道坐标系内的第八投影矢量，以及，第二图像采集装置在巷道坐标系内的第九投影矢量；根据第二投影关系和第三投影关系将第八投影矢量和第九投影矢量转换至捷联惯导壳体坐标系，得到第三投影矢量和第四投影矢量，其中，第二投影关系和第三投影关系为已知量。
本实施例中的激光测距装置优先选用全站仪。摄像头3相对激光靶面上光斑的位移矢量(即上述第二位移矢量)和摄像头4相对激光靶面上光斑的位移矢量(即上述第三位移矢量)在巷道坐标系的投影通过全站仪间接测量。
基于上述实施例，为了标定安装矩阵，采用全站仪对
和
进行间接测量，通过将全站仪置于摄像头3和4与激光靶面之间，采用全站仪即可实现摄像头相对捷联惯导安装角的标定，操作简单，便于使用现场的操作。
在本案的一个可选的实施例中，在根据第一投影矢量、第二投影矢量、第三投影矢量、第四投影矢量和第一投影关系，计算安装矩阵之后，还包括：分别对第一投影矢量、第二投影矢量、第三投影矢量和第四投影矢量进行归一化处理；根据归一化处理结果构造第一图像采集装置的第一归一化矢量和第二图像采集装置的第二归一化矢量，其中，归一化处理结果均为单位正交矩阵；根据第一归一化矢量和第二归一化矢量标定安装矩阵。
通过本实施例对上述第一投影矢量、第二投影矢量、第三投影矢量和第四投影矢量进行归一化处理，即对各个投影矢量的矩阵进行正交化，得到正交矩阵，来计算安装角，可以减小测量误差对角度标定结果的影响。
下面结合一具体实施例对本发明提供的方案做进一步地说明：
图3是根据本发明实施提供的一种掘进机的位姿标定涉及的坐标系标定的示意图，针对图2提供的系统，对系统中各个设备进行坐标系标定。
其中，巷道坐标系t的原点位于激光指向仪上，yt在水平面内沿激光发射方向，zt轴竖直向上，xt、yt和zt构成右手直角坐标系。摄像头4的坐标系c(即上述图像采集装置坐标系)，简称摄像头坐标系，其原点位于摄像头光心，zc沿摄像头光轴方向，xc在焦平面内向右且与zc垂直，xc、yc和zc构成右手直角坐标系。捷联惯导壳体坐标系b固连于捷联惯导壳体，xb轴指向车身右侧，yb轴指向车身前方，zb轴指向车身上方，xb、yb和zb构成右手直角坐标系。地理坐标系n在图3中没有画出，该坐标系原点与b系原点重合，xn指向正东，yn指向正北，zn指向天，xn、yn和zn构成右手直角坐标系。
掘进机需要捷联惯导提供其相对巷道坐标系的位移信息
(即上述位姿信息)，如公式(3)：
其中：
为导航系巷道坐标系n到巷道坐标系t的旋转矩阵，是巷道设计值，属于已知量；
有捷联惯导给出，属于已知量；
为捷联惯导相对摄像头4的位移矢量在惯导壳体系b的投影，属于已知量；
为摄像头4相对激光靶面上光斑的位移矢量r4在摄像头坐标系的投影，由摄像头3和摄像头4构成的双目视觉系统测量；
为摄像头坐标系相对捷联惯导的安装矩阵，必须在系统投入使用前通过标定方法确定。
摄像头3相对激光靶面上光斑的位移矢量r3(第一位移矢量)和摄像头4相对激光靶面上光斑的位移矢量r4(第二位移矢量)不平行，本发明根据这一特性进行摄像头坐标系c相对捷联惯导壳体系b的安装矩阵
的标定。
摄像头3和摄像头4根据双目视觉原理测得矢量r3和r4在摄像头4的坐标系c(图像采集装置坐标系)内的投影
(第一投影矢量)和
(第二投影矢量)。根据r3和r4在c和b系的投影关系得上述公式(1)。在计算得到
和
之后，可计算得到上述公式(2)。
为利用公式(2)标定安装矩阵
采用全站仪对
(即上述第三投影矢量)和
(即上述第四投影矢量)进行间接测量，测量原理见图4，图4是根据本发明实施例提供的安装角标定方法原理示意图。
图4中o为全站仪所在位置，全站仪输出结果为
(即上述第五投影矢量)、
(即上述第六投影矢量)和
(即上述第七投影矢量)，即图3中rA(即上述第三位移矢量)、rB(即上述第四位移矢量)和rC(即上述第五位移矢量)在巷道坐标系的投影。根据矢量合成法则得公式(4)和公式(5)：
进一步利用
和
两个已知量将
(即上述第八投影矢量)和
(即上述第九投影矢量)变换到b系为公式(6)和公式(7)：
将
和
带入公式(1)即可得到
的标定结果。
考虑到
和
都存在测量误差，所以实际标定算法首先将上述测量结果归一化：
然后构造与v3和v4正交的归一化矢量
和
则
和
存在以下关系：
由此得：
综上，本发明工作过程包括如下步骤:
步骤1:根据双目视觉原理测得摄像头3相对激光靶面上光斑的位移矢量r3在摄像头4的坐标系c内的投影
见附图2；
步骤2:根据双目视觉原理测得摄像头4相对激光靶面上光斑的位移矢量r4在摄像头4的坐标系c内的投影
见附图2；
步骤3:将全站仪置于摄像头3和4与激光靶面之间，测量摄像头3、摄像头4和激光靶面上的光斑相对于全站仪的矢量
和
步骤4:计算r3和r4在巷道坐标系的投影
和
步骤5:计算r3和r4在惯导壳体坐标系b的投影
和
步骤6:计算
和
的归一化矢量，得到单位正交矩阵：
步骤7:构造与v3和v4正交的归一化矢量
和
步骤8:计算最终的标定结果
通过本发明实施例至少具有以下有益效果：
(1)实现激光指向仪位置信息引入，替代人工对点；
(2)标定双目视觉摄像头相对捷联惯导的安装角，是激光指向仪位置信息引入的关键；
(3)采用全站仪即可实现摄像头相对捷联惯导安装角的标定，操作简单，便于使用现场的操作；
(4)本发明不需要陀螺寻北仪和平行光管等专业设备，费用低。
基于上文各个实施例提供的掘进机的位姿标定方法，基于同一发明构思，在本实施例中还提供了一种掘进机的位姿标定系统，该系统至少包括掘进机、捷联惯导、第一图像采集装置、第二图像采集装置、激光靶以及激光指向仪，其中，激光靶和激光指向仪均设于巷道内，激光靶悬挂在激光指向仪前侧，激光指向仪用于在激光靶面上形成光斑；第一图像采集装置和第二图像采集装置均设于掘进机上，用于通过采集激光靶上的光斑图像获取第一图像采集装置相对光斑的第一位移矢量以及第二图像采集装置相对光斑的第二位移矢量，其中，第一位移矢量和第二位移矢量空间内不平行；捷联惯导，设于掘进机上，用于根据图像采集装置坐标系相对捷联惯导壳体坐标系的第一投影关系、第一位移矢量和第二位移矢量确定图像采集装置坐标系相对捷联惯导的安装矩阵，其中，安装矩阵用于表征图像采集装置坐标系相对捷联惯导的安装角，以标定捷联惯导在巷道内的位姿信息。
在本案的一个可选的实施例中，该系统还包括激光测距装置，其中，激光测距装置，设于巷道内，且设于第一图像采集装置、第二图像采集装置和激光靶之间，用于测量第一图像采集装置、第二图像采集装置和激光靶面上的光斑相对激光测距装置的位移矢量。
在本案的一个可选的实施例中，激光测距装置为全站仪。
在本案的一个可选的实施例中，激光指向仪发射的光线平行于巷道设定的水平面方向。
在本案的一个可选的实施例中，激光靶安装于掘进机后方巷道顶面或侧面上。
该系统用于实现上述方法实施例及优选实施方式，已经进行过说明的不再赘述。
显然，本领域的技术人员应该明白，上述的本发明的各模块或各步骤可以用通用的计算装置来实现，它们可以集中在单个的计算装置上，或者分布在多个计算装置所组成的网络上，可选地，它们可以用计算装置可执行的程序代码来实现，从而，可以将它们存储在存储装置中由计算装置来执行，并且在某些情况下，可以以不同于此处的顺序执行所示出或描述的步骤，或者将它们分别制作成各个集成电路模块，或者将它们中的多个模块或步骤制作成单个集成电路模块来实现。这样，本发明不限制于任何特定的硬件和软件结合。
以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。
Claims (10)
1.一种掘进机的位姿标定方法，其特征在于，包括：
获取第一图像采集装置相对激光靶面上光斑的第一位移矢量，以及第二图像采集装置相对所述光斑的第二位移矢量，其中，所述第一位移矢量和所述第二位移矢量空间内不平行，所述激光靶悬挂在激光指向仪前侧，所述激光指向仪用于在所述激光靶面上形成所述光斑；
根据所述第一位移矢量、所述第二位移矢量以及图像采集装置坐标系相对捷联惯导壳体坐标系的第一投影关系，确定所述图像采集装置坐标系相对捷联惯导的安装矩阵，其中，所述捷联惯导设于所述掘进机上；
通过所述安装矩阵确定所述图像采集装置坐标系相对所述捷联惯导的安装角，以标定所述捷联惯导在巷道内的位姿信息。
2.根据权利要求1所述的方法，其特征在于，根据所述第一位移矢量、所述第二位移矢量以及图像采集装置坐标系相对捷联惯导壳体坐标系的第一投影关系，确定所述图像采集装置坐标系相对捷联惯导的安装矩阵，包括：
基于双目视觉算法确定所述第一位移矢量在所述图像采集装置坐标系内的第一投影矢量，以及所述第二位移矢量在所述图像采集装置坐标系内的第二投影矢量，其中，所述图像采集装置坐标系是基于所述第一图像采集装置或第二图像采集装置的中心位置为原点、光轴方向为z轴、焦平面且与z垂直方向为x轴构建的空间直角坐标系；
确定所述第一位移矢量在所述捷联惯导壳体坐标系内的第三投影矢量，以及所述第二位移矢量在所述捷联惯导壳体坐标系内的第四投影矢量，其中，所述捷联惯导壳体坐标系是以所述捷联惯导的壳体为原点、掘进机的前进方向为y轴、掘进机竖直面且与y轴竖直方向为z轴构建的空间直角坐标系；
根据所述第一投影矢量、所述第二投影矢量、所述第三投影矢量、所述第四投影矢量和所述第一投影关系，计算所述安装矩阵。
3.根据权利要求2所述的方法，其特征在于，确定所述第一位移矢量在所述捷联惯导壳体坐标系内的第三投影矢量，以及所述第二位移矢量在所述捷联惯导壳体坐标系内的第四投影矢量，包括：
获取所述第一图像采集装置相对激光测距装置的第三位移矢量；以及，获取所述第二图像采集装置相对所述激光测距装置的第四位移矢量；以及，获取所述激光测距装置相对所述光斑的第五位移矢量，其中，所述激光测距装置设于所述巷道内，位于所述第一图像采集装置、所述第二图像采集装置和所述激光靶之间；
分别测量所述第三位移矢量、所述第四位移矢量和所述第五位移矢量在巷道坐标系内对应的第五投影矢量、第六投影矢量和第七投影矢量；
根据所述第五投影矢量、所述第六投影矢量、所述第七投影矢量、地理坐标系相对所述巷道坐标系的第二投影关系、所述捷联惯导壳体坐标系相对所述地理坐标系的第三投影关系，计算所述第三投影矢量和所述第四投影矢量，其中，所述巷道坐标系是以所述激光指向仪为原点、水平面内激光发射方向为y轴、竖直方向为z轴构建的空间直角坐标系。
4.根据权利要求3所述的方法，其特征在于，根据所述第五投影矢量、所述第六投影矢量、所述第七投影矢量、地理坐标系相对所述巷道坐标系的第二投影关系、所述捷联惯导壳体坐标系相对地理坐标系的第三投影关系，计算所述第三投影矢量和所述第四投影矢量，包括：
基于矢量合成法则，根据所述第五投影矢量、所述第六投影矢量和所述第七投影矢量计算所述第一图像采集装置在巷道坐标系内的第八投影矢量，以及，所述第二图像采集装置在巷道坐标系内的第九投影矢量；
根据所述第二投影关系和所述第三投影关系将所述第八投影矢量和所述第九投影矢量转换至捷联惯导壳体坐标系，得到所述第三投影矢量和所述第四投影矢量，其中，所述第二投影关系和所述第三投影关系为已知量。
5.根据权利要求2所述的方法，其特征在于，在根据所述第一投影矢量、所述第二投影矢量、所述第三投影矢量、所述第四投影矢量和所述第一投影关系，计算所述安装矩阵之后，所述方法还包括：
分别对所述第一投影矢量、所述第二投影矢量、所述第三投影矢量和所述第四投影矢量进行归一化处理；
根据归一化处理结果构造所述第一图像采集装置的第一归一化矢量和所述第二图像采集装置的第二归一化矢量，其中，所述归一化处理结果均为单位正交矩阵；
根据所述第一归一化矢量和所述第二归一化矢量标定所述安装矩阵。
6.一种掘进机的位姿标定系统，其特征在于，所述系统至少包括掘进机、捷联惯导、第一图像采集装置、第二图像采集装置、激光靶以及激光指向仪，其中，
所述激光靶和所述激光指向仪均设于巷道内，所述激光靶悬挂在所述激光指向仪前侧，所述激光指向仪用于在所述激光靶面上形成光斑；
所述第一图像采集装置和所述第二图像采集装置均设于所述掘进机上，用于通过采集所述激光靶上的光斑图像获取所述第一图像采集装置相对所述光斑的第一位移矢量以及所述第二图像采集装置相对所述光斑的第二位移矢量，其中，所述第一位移矢量和所述第二位移矢量空间内不平行；
所述捷联惯导，设于所述掘进机上，用于根据图像采集装置坐标系相对捷联惯导壳体坐标系的第一投影关系、所述第一位移矢量和所述第二位移矢量确定所述图像采集装置坐标系相对所述捷联惯导的安装矩阵，其中，所述安装矩阵用于表征所述图像采集装置坐标系相对所述捷联惯导的安装角，以标定所述捷联惯导在所述巷道内的位姿信息。
7.根据权利要求6所述的系统，其特征在于，所述系统还包括激光测距装置，其中，
所述激光测距装置，设于所述巷道内，且设于所述第一图像采集装置、所述第二图像采集装置和所述激光靶之间，用于测量所述第一图像采集装置、所述第二图像采集装置和所述激光靶面上的光斑相对所述激光测距装置的位移矢量。
8.根据权利要求7所述的系统，其特征在于，所述激光测距装置为全站仪。
9.根据权利要求6所述的系统，其特征在于，所述激光指向仪发射的光线平行于所述巷道设定的水平面方向。
10.根据权利要求6所述的系统，其特征在于，所述激光靶安装于所述掘进机后方巷道顶面或侧面上。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011531231.7A CN112665613A (zh) | 2020-12-22 | 2020-12-22 | 掘进机的位姿标定方法及系统 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011531231.7A CN112665613A (zh) | 2020-12-22 | 2020-12-22 | 掘进机的位姿标定方法及系统 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN112665613A true CN112665613A (zh) | 2021-04-16 |
Family
ID=75407741
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202011531231.7A Pending CN112665613A (zh) | 2020-12-22 | 2020-12-22 | 掘进机的位姿标定方法及系统 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN112665613A (zh) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113534824A (zh) * | 2021-07-21 | 2021-10-22 | 中国科学院沈阳自动化研究所 | 面向水下机器人集群的视觉定位与近距离密集编队方法 |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105241444A (zh) * | 2015-09-19 | 2016-01-13 | 中国煤炭科工集团太原研究院有限公司 | 一种悬臂式掘进机空间位姿自动检测系统及其测量方法 |
| WO2018214988A1 (zh) * | 2017-05-25 | 2018-11-29 | 中国矿业大学 | 一种掘进机绝对空间位姿检测装置与方法 |
| CN109115173A (zh) * | 2018-08-23 | 2019-01-01 | 西安科技大学 | 基于直线定位模型的掘进机机身位姿单目视觉测量方法 |
| CN111535810A (zh) * | 2020-05-06 | 2020-08-14 | 三一重型装备有限公司 | 姿态检测设备和掘进机 |
| CN111780748A (zh) * | 2020-05-16 | 2020-10-16 | 北京航天众信科技有限公司 | 基于双目视觉和捷联惯导的掘进机位姿纠偏方法、系统 |
-
2020
- 2020-12-22 CN CN202011531231.7A patent/CN112665613A/zh active Pending
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105241444A (zh) * | 2015-09-19 | 2016-01-13 | 中国煤炭科工集团太原研究院有限公司 | 一种悬臂式掘进机空间位姿自动检测系统及其测量方法 |
| WO2018214988A1 (zh) * | 2017-05-25 | 2018-11-29 | 中国矿业大学 | 一种掘进机绝对空间位姿检测装置与方法 |
| CN109115173A (zh) * | 2018-08-23 | 2019-01-01 | 西安科技大学 | 基于直线定位模型的掘进机机身位姿单目视觉测量方法 |
| CN111535810A (zh) * | 2020-05-06 | 2020-08-14 | 三一重型装备有限公司 | 姿态检测设备和掘进机 |
| CN111780748A (zh) * | 2020-05-16 | 2020-10-16 | 北京航天众信科技有限公司 | 基于双目视觉和捷联惯导的掘进机位姿纠偏方法、系统 |
Non-Patent Citations (1)
| Title |
|---|
| 呼守信;: "基于惯性测量的悬臂式掘进机位姿自动定位", 机电产品开发与创新, no. 03, 28 May 2017 (2017-05-28) * |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113534824A (zh) * | 2021-07-21 | 2021-10-22 | 中国科学院沈阳自动化研究所 | 面向水下机器人集群的视觉定位与近距离密集编队方法 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| KR100728377B1 (ko) | 레이저 스캐너 및 무선인터넷을 이용한 변경된 지역시설물의 ｇｉｓ 실시간 업데이트 방법 | |
| US10509983B2 (en) | Operating device, operating system, operating method, and program therefor | |
| US9322652B2 (en) | Stereo photogrammetry from a single station using a surveying instrument with an eccentric camera | |
| EP1607718B1 (en) | Surveying instrument and electronic storage medium | |
| JP4607095B2 (ja) | 測量機器において画像処理を行う方法と装置 | |
| CN113140040B (zh) | 一种多传感器融合的煤矿井下空间定位及建图方法与装置 | |
| EP1655573B1 (en) | 3-dimensional measurement device and electronic storage medium | |
| CN113820735B (zh) | 位置信息的确定方法、位置测量设备、终端及存储介质 | |
| CN105424058B (zh) | 基于摄影测量技术的数码相机投影中心位置精确标定方法 | |
| CN108648242B (zh) | 基于激光测距仪辅助无公共视场的两相机标定方法及装置 | |
| CN102575933A (zh) | 地图图像综合数据库生成系统以及地图图像综合数据库生成程序 | |
| CN105378794A (zh) | 3d拍摄装置、用于建立3d图像的方法和用于构建3d拍摄装置的方法 | |
| CN216116064U (zh) | 掘进机的位姿标定系统 | |
| CN111780748A (zh) | 基于双目视觉和捷联惯导的掘进机位姿纠偏方法、系统 | |
| CN117434570B (zh) | 坐标的可视化测量方法、测量设备及存储介质 | |
| CN113140039B (zh) | 一种多传感器融合的煤矿井下数字化定位及地图构建系统 | |
| CN113340272B (zh) | 一种基于无人机微群的地面目标实时定位方法 | |
| CN112665613A (zh) | 掘进机的位姿标定方法及系统 | |
| CN111623821B (zh) | 隧道钻孔方向的检测、偏差检测、钻孔位置确定的方法 | |
| JP5886241B2 (ja) | 携帯型撮影装置 | |
| CN117437370A (zh) | 基于混合现实三维建模的高压电缆定位方法、装置及设备 | |
| CN114415155B (zh) | 单点激光测距仪与可见光相机的位置标定方法 | |
| US20090087013A1 (en) | Ray mapping | |
| CN108322698B (zh) | 基于多摄像机和惯性测量单元融合的系统和方法 | |
| EP2696168A1 (en) | Using gravity measurements within a photogrammetric adjustment |
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 |