[发明专利]一种地下空间内既定路径自主定位方法

权利要求书

1.一种地下空间内既定路径自主定位方法，其特征在于：该方法为：

利用地下空间的分层特性，将地下空间分解为多个一定深度的二维平面，针对地下空间巷道分布数字化平面地图结合巷道交叉点坐标，设备E₀自主定位变化为既定路径的惯性导航，分解为简单的直线运动导航和交叉点标定，计算出实时相对坐标和运动路径。

2.根据权利要求1所述的一种地下空间内既定路径自主定位方法，其特征在于：所述方法具体包括以下步骤：

S1：结合电子地图标注出所有的巷道交叉点及坐标，建立交叉点坐标矩阵并导入设备，建立所有运动路径的数学模型，任意路径成为既定路径；

S2：设备运动过程中当姿态角变化超过角度阈值时判定交叉点；

S3：设备运动到巷道交叉点时直接用交叉点已知坐标标定实时坐标，消除惯性导航累计误差。

3.根据权利要求2所述的一种地下空间内既定路径自主定位方法，其特征在于：所述地下空间巷道分布数字化平面地图中，起始点O(0,0)作为平面坐标原点，将所有巷道交叉连接点逐点逐行编号，并标明平面内相对坐标，包括A₁(x₁,y₁)至A_n(x_n,y_n)，根据交叉点坐标确定巷道内所有路径，将地下空间任意一条路径明确为既定路径，地下空间设备自主定位变化为一定深度值条件下的二维既定路径惯性导航；

A₁(x₁,y₁)至A_n(x_n,y_n)按照行列关系生成二维数字化平面地图坐标矩阵如下式：

每行元素个数由同一水平巷道交叉点数量确定，其余各行不够的元素空缺，行数由电子地图中水平巷道数量确定。

4.根据权利要求3所述的一种地下空间内既定路径自主定位方法，其特征在于：所述设备E₀的工作过程分为以下三个步骤：

第一步零速标定：在起始点O(0,0)处设备E₀正对水平巷道中心静止状态开始标定初始状态和初始姿态，设备E₀初始化当前坐标为原点O(0,0)，初始姿态为正向面对X轴正向；

第二步交叉点标定：设备E₀运动到所有的A₁(x₁,y₁)到A_n(x_n,y_n)处，即设备E₀姿态角水平方向变化值超过阈值θ_th时判定为设备E₀到达指定交叉点A_i(x_i,y_i)，根据交叉点已知坐标值对当前实时值进行校正，消除惯性导航的累计误差；

第三步直线惯性导航：设备E₀直线运动，即设备E₀姿态角水平方向变化值不超过阈值θ_th时，经过惯性导航组件解算得到设备E₀相对导航坐标系下的速度、位置和姿态角的信息计算出实时坐标和运动路径。