CN114485582B - 基于全站仪测量的挖掘机关键点的角度及距离的标定方法 - Google Patents

Abstract

本发明公开了一种基于全站仪测量的挖掘机关键点的角度及距离的标定方法，属于测绘应用和挖掘机技术领域，包括挖掘机、全站仪、计算机、挖掘机关键点标定算法，通过全站仪测量挖掘机上的挖掘机关键点，在计算机上的挖掘机关键点标定算法进行计算，通过几何建模的方法对复杂的挖掘机关键点归结到同一竖直面中进行计算，计算出挖掘机关键点之间的角度和距离信息，从而对挖掘机整机挖掘机关键点进行标定，解决了挖掘机由于机械结构复杂而无法准确测量其挖掘机关键点的问题，可以提高挖掘机后装传感器的安装精度及传感器参数校准。

Description

基于全站仪测量的挖掘机关键点的角度及距离的标定方法

技术领域

本发明属于测绘应用和工程机械领域，具体涉及基于全站仪测量的挖掘机关键点的角度及距离的标定方法。

背景技术

为了提高生产效率、降低作业难度、提高作业精准度，挖掘机的辅助施工系统在挖掘机施工中的使用越来越广泛，而辅助施工系统的基础是传感器等设备，传感器等设备在安装的时候，需要安装在固定的点上，并且还需要挖掘机上一些特殊的挖掘机关键点之间的距离及角度关系来设置参数或者校准等。挖掘机的前装传感器等设备在安装的时候由于有具体的设备图纸的情况下，在忽略安装误差的情况下可以直接由图纸得出传感器安装的相关参数；而在后装传感器等设备的时候，由于没有设备图纸，并且在挖掘机的机械结构较为复杂的情况下，各个挖掘机关键点都不处于同一面内，无法直接测量出挖掘机关键点在空间中的信息。

现有的标定方法大多采用从挖掘机的动臂与平台的铰接处做一条与水平地面平行的水平线，根据所述水平线控制挖掘机的动臂与斗杆的铰接处处于所述水平线上，对动臂的的角度传感器进行标定；再从挖掘机的动臂与斗杆的铰接处做一条与所述水平地面垂直的垂线，控制挖掘机的斗杆与铲斗的铰接处处于所述垂线上；从挖掘机的摇杆与连杆的绞点处做一条与所述水平地面垂直的垂线；根据所述垂线控制挖掘机的斗杆与摇杆的绞点处于所述垂线上，对摇杆角度传感器进行标定。解决了挖掘机动臂、斗杆、摇杆的角度传感器的安装标定。

上述方法通过人工设置水平面，具有一定的误差；需要挖掘机的不断配合着与所设线对齐，造作不便利；在挖掘机改变姿态的时候，挖掘机自身的误差会累积体现，增大最后的测量误差；在挖掘机的工作环境中，不便寻找合适的测量平台；只对挖掘机的动臂等施工部分进行标定，无法对车身上安装的传感器位置进行标定，缺乏了灵活性和兼容性；需要测量人员处于挖掘机的工作臂下，具有一定的危险。

发明内容

本发明公开了基于全站仪测量的挖掘机关键点的角度及距离的标定方法，解决现有技术中挖掘机存在测量误差、不适合寻找测量平台的问题。

基于全站仪测量的挖掘机关键点的角度及距离的标定方法，包括：

S1.建立三维空间标定坐标系，通过全站仪测量出全站仪与挖掘机设置的挖掘机关键点之间的连线的距离、连线与水平面之间的夹角以及各连线之间水平投影的夹角，以此建立空间三维坐标系，得到挖掘机各个挖掘机关键点的空间三维坐标；

S2.根据空间中点投影于面几何计算原理，将复杂的挖掘机关键点向过挖掘机斗杆的全站仪架设一侧的竖直基准面投影，通过几何方法解决复杂的挖掘机关键点计算问题；

S3.归结挖掘机关键点三维坐标点于同一个竖直基准面，将挖掘机关键点投影于竖直基准面后，得到在该竖直基准面内的三维坐标；

S4.标定点数据处理，将测量数据从全站仪导入到计算机上，在计算机上运行自主开发的挖掘机关键点标定算法程序，计算挖掘机关键点之间的角度和距离，最终完成标定。

优选地，步骤S1包括以下子步骤：

S1.1.对所需的挖掘机关键点使用英文字母进行标注；

S1.2.挖掘机停在一个平面上，在挖掘机一侧架设全站仪；

S1.3.通过全站仪对挖掘机上各个挖掘机关键点的测量，得到相关角度及距离信息，将其记录；

S1.4.对测量信息初步处理，对数据进行改写。

优选地，进行步骤S1.2的时候需要满足：

挖掘机停靠时需要熄火，保持平稳；架设全站仪要稳固，户外作业的时候全站仪不可出现移动；全站仪架设方向固定，在挖掘机的动臂与上车部分连接销轴能露出一侧；全站仪架设位置与挖掘机保持10-20m的距离。

优选地，所述步骤S1.3包括：

S1.3.1.测量挖掘机关键点连线的长度、连线与水平面的夹角和连线与连线之间的夹角，以全站仪到挖掘机的动臂与上车部分连接销轴中心的连线PA为夹角计算基准线，距离单位为m，角度采用度分秒形式；

S1.3.2.将S1.3.1完成的角度信息进行改写，将大于180°的角度改为小于180°的角度，公式为：β＝360°0′0″-α，β：改正后角度，α：全站仪测量后大于180°角度。

优选地，所述步骤S4包括：

S4.1.读取全站仪测量数据，取得所有挖掘机关键点与全站仪连线的距离PT_lenth、与水平面的夹角、与基准线PA的夹角，T代表所述挖掘机关键点；将测量数据中的角度值由度分秒的形式换算为弧度制，公式为：设度分秒形式为：A°B′C″，β：改正后的弧度制角度，A：测量数据中度的数值，B：测量数据中分的数值，C：测量数据中秒的数值；

上述过程获得所有挖掘机关键点相关角度值的弧度制角度，并存放在：表示挖掘机关键点与全站仪连线与水平面的弧度制夹角T_Pitch和各挖掘机关键点连线与基准线PA的弧度制夹角AT_Horizontal中。

优选地，所述步骤S4包括：

S4.2.建立空间直角坐标系，以全站仪为坐标原点，挖掘机动臂与上车部分的连接销轴中心与全站仪的连线PA在过全站仪水平面水平投影作为x轴，以全站仪到销轴中心为正方向；过全站仪点，并垂直于于x轴在全站仪水平面建立y轴，以向右侧为正方向；过全站仪点，垂直于全站仪水平面为z轴，竖直向上为正方向；

将所有挖掘机关键点与全站仪之间的连线投影到全站仪坐在水平面，称为连线临时值，记作PT_t；PT_t＝PT_lenth*cos(T_Pitch)，PA连线的投影PA_t在x轴上，其y坐标为0，其x和z坐标与其他挖掘机关键点求取方法一致；

各个挖掘机关键点坐标求取公式为：T_x＝PT_t*cos(AT_Horizontal),T_y＝PT_t*sin(AT_Horizontal),T_z＝PT_lenth*sin(T_pitch)，将各挖掘机关键点所求得的坐标保存与三维数组中，保存顺序为(x,y,z)。

优选地，所述步骤S4包括：

S4.3.求得过挖掘机斗杆上C1、C2基准点的竖直基准面，通过交面式求得过C1、C2的两个面，向量C1C2为： 由第一部分和第二部分构成交面式求直线方程的第一个面：

由第二部分和第三部分构成交面式求直线方程的第二个面：

求C1、C2确定的竖直平面，设：

上述为过直线C1C2的平面束方程，取该面的方向向量，与水平面的法向量垂直，可得：

该面的方向向量，/>水平面的法向量，由：/>可得：/>由于C1C2不与水平面平行，则μ＝0，竖直平面为：

一个面方程的变量系数不能全为0，则λ不为0；竖直平面为：

记为：Ax+By+Cz+D＝0。

优选地，所述步骤S4包括：

S4.4.将除C1、C2点外的挖掘机关键点对竖直基准面进行投影，点T(T_x T_y T_z)与其投影点的连线构成的向量与竖直平面的法向量/>平行，直线TTⁱ的参数方程为：/>将上式代入平面方程可得：/>再将t代入直线的参数方程可以得到投影点的坐标为：/>

由此可得投影点坐标为至此将挖掘机所有挖掘机关键点的坐标都投影到挖掘机斗杆上基准点C1、C2所确定的竖直基准面上，并得到投影后的三维坐标。

优选地，所述步骤S4包括：

S4.5.得到挖掘机关键点的投影之后，将所需的距离和角度参数对应的挖掘机关键点构成在竖直基准面内的空间向量，通过空间向量的求模公式和求向量角度的公式得到角度和距离参数。

与现有技术相比，本发明具有较强的兼容性，在传感器安装位置不确定的时候，或者随机设置挖掘机关键点的时候，或者在车身上设置挖掘机关键点等情况下，通过调整自主开发的挖掘机关键点标定算法程序都可以完成完整的计算；对测量环境要求低，不需要寻找特定的测量环境，降低了测量的需求，具有更强的适应性；具有很高的测量精度，由于整套方法依靠全站仪测量得出的高精度数据，转化为几何问题进行计算，没有精度损失；测量过程中便利、快捷，在挖掘机停好的情况下，一个人即可完成整套测量；测量过程的安全性更高，测量人员不需要近距离接触挖掘机，避免了很多测量时的危险，极大的提高了测量过程中的安全系数。

附图说明

图1为挖掘机关键点标注示意图；

图2为全站仪架设示意图；

图3为建立空间直角坐标系第一示意图；

图4为建立空间直角坐标系第二示意图；

图5为以B点为例计算对应变量示意图；

图6为过C1、C2建立以斗杆为基准的竖直基准面。

附图标记包括：A-动臂与上车部分连接销轴中心点；B-动臂与斗杆连接销轴中心点；C1-以斗杆的一面为基准建立基准面的基准点1；C2-以斗杆的一面为基准建立基准面的基准点2；D-摇杆与斗杆连接销轴中心点；E-摇杆与铲斗液压缸连接销轴中心点；G-斗杆与铲斗连接销轴中心点；H-铲斗与摇杆连接销轴中心点；J-斗齿齿尖；K-安装于挖掘机引擎盖上方的定位天线；O-回转平台侧面中心点；P-全站仪观测点。

具体实施方式

下面，通过示例性的实施方式对本发明进行具体描述。然而应当理解，在没有进一步叙述的情况下，一个实施方式中的元件、结构和特征也可以有益地结合到其他实施方式中。

基于全站仪测量的挖掘机关键点的角度及距离的标定方法，包括：

S1.建立三维空间标定坐标系，通过全站仪测量出全站仪与挖掘机设置的挖掘机关键点之间的连线的距离、连线与水平面之间的夹角以及各连线之间水平投影的夹角，以此建立空间三维坐标系，得到挖掘机各个挖掘机关键点的空间三维坐标；

S2.根据空间中点投影于面几何计算原理，将复杂的挖掘机关键点向过挖掘机斗杆的全站仪架设一侧的竖直基准面投影，通过几何方法解决复杂的挖掘机关键点计算问题；

S3.归结挖掘机关键点三维坐标点于同一个竖直基准面，将挖掘机关键点投影于竖直基准面后，得到在该竖直基准面内的三维坐标；

S4.标定点数据处理，将测量数据从全站仪导入到计算机上，在计算机上运行自主开发的挖掘机关键点标定算法程序，计算挖掘机关键点之间的角度和距离，最终完成标定。

步骤S1包括以下子步骤：

S1.1.对所需的挖掘机关键点使用英文字母进行标注；

S1.2.挖掘机停在一个平面上，在挖掘机一侧架设全站仪；

S1.3.通过全站仪对挖掘机上各个挖掘机关键点的测量，得到相关角度及距离信息，将其记录；

S1.4.对测量信息初步处理，对数据进行改写。

进行步骤S1.2的时候需要满足：

挖掘机停靠时需要熄火，保持平稳；架设全站仪要稳固，户外作业的时候全站仪不可出现移动；全站仪架设方向固定，在挖掘机的动臂与上车部分连接销轴能露出一侧；全站仪架设位置与挖掘机保持10-20m的距离。

所述步骤S1.3包括：

S1.3.1.测量挖掘机关键点连线的长度、连线与水平面的夹角和连线与连线之间的夹角，以全站仪到挖掘机的动臂与上车部分连接销轴中心的连线PA为夹角计算基准线，距离单位为m，角度采用度分秒形式；

S1.3.2.将S1.3.1完成的角度信息进行改写，将大于180°的角度改为小于180°的角度，公式为：β＝360°0′0″-α，β：改正后角度，α：全站仪测量后大于180°角度。

所述步骤S4包括：

S4.1.读取全站仪测量数据，取得所有挖掘机关键点与全站仪连线的距离PT_lenth、与水平面的夹角、与基准线PA的夹角，T代表所述挖掘机关键点；将测量数据中的角度值由度分秒的形式换算为弧度制，公式为：设度分秒形式为：A°B′C″，β：改正后的弧度制角度，A：测量数据中度的数值，B：测量数据中分的数值，C：测量数据中秒的数值；

上述过程获得所有挖掘机关键点相关角度值的弧度制角度，并存放在：表示挖掘机关键点与全站仪连线与水平面的弧度制夹角T_Pitch和各挖掘机关键点连线与基准线PA的弧度制夹角AT_Horizontal中。

所述步骤S4包括：

S4.2.建立空间直角坐标系，以全站仪为坐标原点，挖掘机动臂与上车部分的连接销轴中心与全站仪的连线PA在过全站仪水平面水平投影作为x轴，以全站仪到销轴中心为正方向；过全站仪点，并垂直于于x轴在全站仪水平面建立y轴，以向右侧为正方向；过全站仪点，垂直于全站仪水平面为z轴，竖直向上为正方向；

将所有挖掘机关键点与全站仪之间的连线投影到全站仪坐在水平面，称为连线临时值，记作PT_t；PT_t＝PT_lenth*cos(T_Pitch)，PA连线的投影PA_t在x轴上，其y坐标为0，其x和z坐标与其他挖掘机关键点求取方法一致；

各个挖掘机关键点坐标求取公式为：T_x＝PT_t*cos(AT_Horizontal),T_y＝PT_t*sin(AT_Horizontal),T_z＝PT_lenth*sin(T_pitch)，将各挖掘机关键点所求得的坐标保存与三维数组中，保存顺序为(x,y,z)。

所述步骤S4包括：

S4.3.求得过挖掘机斗杆上C1、C2基准点的竖直基准面，通过交面式求得过C1、C2的两个面，向量C1C2为： 由第一部分和第二部分构成交面式求直线方程的第一个面：

由第二部分和第三部分构成交面式求直线方程的第二个面：

求C1、C2确定的竖直平面，设：

上述为过直线C1C2的平面束方程，取该面的方向向量，与水平面的法向量垂直，可得：

该面的方向向量，/>水平面的法向量，由：/>可得：/>由于C1C2不与水平面平行，则μ＝0，竖直平面为：

一个面方程的变量系数不能全为0，则λ不为0；竖直平面为：

记为：Ax+By+Cz+D＝0。

所述步骤S4包括：

S4.4.将除C1、C2点外的挖掘机关键点对竖直基准面进行投影，点T(T_x T_y T_z)与其投影点的连线构成的向量与竖直平面的法向量/>平行，直线TTⁱ的参数方程为：/>将上式代入平面方程可得：/>再将t代入直线的参数方程可以得到投影点的坐标为：/>

由此可得投影点坐标为至此将挖掘机所有挖掘机关键点的坐标都投影到挖掘机斗杆上基准点C1、C2所确定的竖直基准面上，并得到投影后的三维坐标。

所述步骤S4包括：

S4.5.得到挖掘机关键点的投影之后，将所需的距离和角度参数对应的挖掘机关键点构成在竖直基准面内的空间向量，通过空间向量的求模公式和求向量角度的公式得到角度和距离参数。

以上所举实施例仅用为方便举例说明本发明，并非对本发明保护范围的限制，在本发明所述技术方案范畴，所属技术领域的技术人员所作各种简单变形与修饰，均应包含在以上申请专利范围中。

当然，上述说明并非是对本发明的限制，本发明也并不仅限于上述举例，本技术领域的技术人员在本发明的实质范围内所做出的变化、改型、添加或替换，也应属于本发明的保护范围。

Claims (1)

1.基于全站仪测量的挖掘机关键点的角度及距离的标定方法，其特征在于，包括：

S1.建立三维空间标定坐标系，通过全站仪测量出全站仪与挖掘机设置的挖掘机关键点之间的连线的距离、连线与水平面之间的夹角以及各连线之间水平投影的夹角，以此建立空间三维坐标系，得到挖掘机各个挖掘机关键点的空间三维坐标；

S2.根据空间中点投影于面几何计算原理，将挖掘机关键点向过挖掘机斗杆的全站仪架设一侧的竖直基准面投影，通过几何方法解决挖掘机关键点计算问题；

S3.归结挖掘机关键点三维坐标点于同一个竖直基准面，将挖掘机关键点投影于竖直基准面后，得到在该竖直基准面内的三维坐标；

S4.标定点数据处理，将测量数据从全站仪导入到计算机上，在计算机上运行挖掘机关键点标定算法程序，计算挖掘机关键点之间的角度和距离，最终完成标定；

步骤S1包括以下子步骤：

S1.1.对所需的挖掘机关键点使用字母进行标注；

S1.2.挖掘机停在一个平面上，在挖掘机一侧架设全站仪；

S1.3.通过全站仪对挖掘机上各个挖掘机关键点的测量，得到相关角度及距离信息，将其记录；

S1.4.对测量信息初步处理，对数据进行改写；进行步骤S1.2的时候需要满足：

挖掘机停靠时需要熄火，保持平稳；架设全站仪要稳固，户外作业的时候全站仪不可出现移动；全站仪架设方向固定，在挖掘机的动臂与上车部分连接销轴能露出一侧；全站仪架设位置与挖掘机保持10-20m的距离；所述步骤S1.3包括：

S1.3.1.测量挖掘机关键点连线的长度、连线与水平面的夹角和各连线之间的夹角，以全站仪到挖掘机的动臂与上车部分连接销轴中心的连线PA为夹角计算基准线，距离单位为m，角度采用度分秒形式；

S1.3.2.将S1.3.1完成的角度信息进行改写，将大于180°的角度改为小于180°的角度，公式为：β＝360°0′0″-α，β：改正后角度，α：全站仪测量后大于180°角度；

所述步骤S4包括：

S4.1.读取全站仪测量数据，取得所有挖掘机关键点与全站仪连线的距离PT_lenth、与水平面的夹角、与夹角计算基准线PA的夹角，T代表所述挖掘机关键点；将测量数据中的角度值由度分秒的形式换算为弧度制，公式为：设度分秒形式为：A°B′C″， β：改正后的弧度制角度，A：测量数据中度的数值，B：测量数据中分的数值，C：测量数据中秒的数值；

上述过程获得所有挖掘机关键点相关角度值的弧度制角度，并存放在：表示挖掘机关键点与全站仪连线与水平面的弧度制夹角T_Pitch和各挖掘机关键点连线与夹角计算基准线PA的弧度制夹角AT_Horizontal中；所述步骤S4包括：

S4.2.建立空间直角坐标系，以全站仪为坐标原点，挖掘机动臂与上车部分的连接销轴中心与全站仪的连线PA在过全站仪水平面水平投影作为x轴，以全站仪到销轴中心为正方向；过全站仪点，并垂直于x轴在全站仪水平面建立y轴，以向右侧为正方向；过全站仪点，垂直于全站仪水平面为z轴，竖直向上为正方向；

将所有挖掘机关键点与全站仪之间的连线投影到全站仪所在水平面，称为连线临时值，记作PT_t；PT_t＝PT_lenth*cos(T_Pitch)，PA连线的投影PA_t在x轴上，其y坐标为0，其x和z坐标与其他挖掘机关键点求取方法一致；

各个挖掘机关键点坐标求取公式为：T_x＝PT_t*cos(AT_Horizontal),T_y＝PT_t*sin(AT_Horizontal),T_z＝PT_lenth*sin(T_pitch)，将各挖掘机关键点所求得的坐标保存与三维数组中，保存顺序为(T_x,T_y,T_z)；

所述步骤S4包括：

S4.3.求得过挖掘机斗杆上C1、C2基准点的竖直基准面，通过交面式求得过C1、C2的两个面，向量C1C2为： 由第一部分和第二部分构成交面式求直线方程的第一个面：

由第二部分和第三部分构成交面式求直线方程的第二个面：

求C1、C2确定的竖直平面，设：

上述为过直线C1C2的平面束方程，取该面的方向向量，与水平面的法向量垂直，可得：

该面的方向向量，/>水平面的法向量，由：/>可得：/>由于C1C2不与水平面平行，则μ＝0，竖直平面为：

一个面方程的变量系数不能全为0，则λ不为0；竖直平面为：

记为：Ax+By+Cz+D＝0；所述步骤S4包括：

S4.4.将除C1、C2点外的挖掘机关键点对竖直基准面进行投影，点T(T_x T_y T_z)与其投影点的连线构成的向量与竖直平面的法向量/>平行，直线TTⁱ的参数方程为：将上式代入竖直平面方程可得：/>再将t代入直线的参数方程得到投影点的坐标为：/>

由此得投影点坐标为至此将挖掘机所有挖掘机关键点的坐标都投影到挖掘机斗杆上基准点C1、C2所确定的竖直基准面上，并得到投影后的三维坐标；所述步骤S4包括：

S4.5.得到挖掘机关键点的投影之后，将所需的距离和角度参数对应的挖掘机关键点构成在竖直基准面内的空间向量，通过空间向量的求模公式和求向量角度的公式得到角度和距离参数。