[发明专利]一种掘进机车体及其截割滚筒的定位方法及定位系统

权利要求书

1.一种掘进机车体及其截割滚筒的定位方法，其特征在于，包括：

计算全站仪坐标系B转换为地理坐标系D的转换系数T_{（地到站）}；

计算台车在地理坐标系D下的坐标（X_车，Y_车，Z_车），测得所述台车绕X轴的旋转角度α_车、绕Y轴旋转的角度β_车，并计算得到所述台车绕Z轴旋转的角度γ_车，实现所述台车在地理坐标系D下的六参数X_车，Y_车，Z_车，α_车，β_车，γ_车精准定位；

计算获得截割滚筒（6）的伸缩坐标与俯仰坐标在地理坐标系D下的坐标，得出所述截割滚筒（6）的位姿；

所述计算台车在地理坐标系D下的坐标（X_车，Y_车，Z_车），测得台车绕X轴的旋转角度α_车、绕Y轴旋转的角度β_车，并计算得到台车绕Z轴旋转的角度γ_车，包括：

获取台车坐标系C下第一棱镜（3）的零点坐标（X₀₁，Y₀₁，Z₀₁）、第二棱镜（4）的零点坐标（X₀₂，Y₀₂，Z₀₂）以及台车绕X轴的旋转角度α_车、绕Y轴旋转的角度β_车；

获取所述第一棱镜（3）在全站仪坐标系B下的实时坐标（X₁，Y₁，Z₁）以及所述第二棱镜（4）在全站仪坐标系B下的实时坐标（X₂，Y₂，Z₂），并根据地理坐标系D与全站仪坐标系B的转换关系，求得所述第一棱镜（3）与所述第二棱镜（4）在地理坐标系D下的坐标；

根据所述第一棱镜（3）的零点坐标（X₀₁，Y₀₁，Z₀₁）、所述第二棱镜（4）的零点坐标（X₀₂，Y₀₂，Z₀₂）与台车坐标系C的关系，计算得出所述台车在地理坐标系D下的坐标（X_车，Y_车，Z_车）；

根据所述第一棱镜（3）的零点坐标（X₀₁，Y₀₁，Z₀₁）、所述第二棱镜（4）的零点坐标（X₀₂，Y₀₂，Z₀₂）、所述第一棱镜（3）在全站仪坐标系B下的坐标（X₁，Y₁，Z₁）、所述第二棱镜（4）在全站仪坐标系B下的坐标（X₂，Y₃，Z₂）以及所述台车在地理坐标系D下的坐标（X_车，Y_车，Z_车）计算得到所述台车绕Z轴旋转的角度γ_车。