[发明专利]一种基于惯性导航数据的采煤机自主定位方法及装置

权利要求书

1.一种基于惯性导航数据的采煤机自主定位方法，其特征在于，所述方法包括：

根据安装在采煤机上的惯性传感仪，在载体坐标系下测量得到采煤机的第一定位数据，其中所述第一定位数据包括：采煤机在载体坐标系下的平移加速度和采煤机在载体坐标系下的旋转角速度值；

在载体坐标系下构建第一姿态矩阵，并进行微分得到导航坐标系下的第二姿态矩阵；

对所述第一定位数据中的所述平移加速度结合采煤机的速度和位置的初始估值进行积分计算得到导航坐标系下的第二定位数据；

根据所述第二姿态矩阵进行逆运算得到采煤机在导航坐标系下的姿态角；

根据所述第二定位数据和所述姿态角确定采煤机在所述导航坐标系下的位置；

所述在载体坐标系下构建第一姿态矩阵，包括：

通过采煤机的初态航向角、俯仰角和横滚角构建第一姿态矩阵，通过如下公式构建第一姿态矩阵：其中表示n系与b系之间旋转角度，分别表示n系与b系之间的三维坐标系下的转轴方向；设u是n系中的旋转矢量，载体绕u旋转，旋转角度为β，l，m，n分别表示为u在n系中三个转轴方向的投影；n表示导航坐标系；b表示载体坐标系；

所述对所述第一姿态矩阵进行微分包括：所述第一姿态矩阵和所述载体坐标系下的旋转角速度值的乘积作为所述微分的结果。

2.根据权利要求1所述的基于惯性导航数据的采煤机自主定位方法，其特征在于，

所述第二定位数据包括：采煤机在导航坐标系下的速度值和采煤机在载体坐标系下的位移值。

3.根据权利要求1所述的基于惯性导航数据的采煤机自主定位方法，其特征在于，所述惯性传感仪包括：用于测量所述第一定位数据的平移加速度的摆式加速度计和用于测量所述第一定位数据的旋转角速度值的激光陀螺仪。

4.根据权利要求2或3所述的基于惯性导航数据的采煤机自主定位方法，其特征在于，在所述对所述第一定位数据中的所述平移加速度结合采煤机的速度和位置的初始估值进行积分计算得到导航坐标系下的第二定位数据之前，所述方法还包括：

利用扩展卡尔曼滤波对所述第一定位数据的旋转角速度和平移加速度进行误差补偿。

5.根据权利要求1所述的基于惯性导航数据的采煤机自主定位方法，其特征在于，将微分后得到所述第二姿态矩阵中的方向余弦元素作为矩阵中的元素对所述第一姿态矩阵中的元素进行更新，采用更新后的第一姿态矩阵结合采煤机的速度和位置的初始估值进行积分计算得到导航坐标系下的第二定位数据。

6.一种基于惯性导航数据的采煤机自主定位装置，其特征在于，所述装置包括：

测量模块，用于根据安装在采煤机上的惯性传感仪，在载体坐标系下测量得到采煤机的第一定位数据，其中所述第一定位数据包括：采煤机在载体坐标系下的平移加速度和采煤机在载体坐标系下的旋转角速度值；

微分运算模块，用于在载体坐标系下构建第一姿态矩阵，并进行微分得到导航坐标系下的第二姿态矩阵；

积分运算模块，用于对所述第一定位数据中的所述平移加速度结合采煤机的速度和位置的初始估值进行积分计算得到导航坐标系下的第二定位数据；

逆运算模块，用于根据所述第二姿态矩阵进行逆运算得到采煤机在导航坐标系下的姿态角；

位置确定模块，用于根据所述第二定位数据和所述姿态角确定采煤机在所述导航坐标系下的位置；

所述微分运算模块中所述在载体坐标系下构建第一姿态矩阵，包括：

通过采煤机的初态航向角、俯仰角和横滚角构建第一姿态矩阵，通过如下公式构建第一姿态矩阵：其中表示n系与b系之间旋转角度，分别表示n系与b系之间的三维坐标系下的转轴方向；设u是n系中的旋转矢量，载体绕u旋转，旋转角度为β，l，m，n分别表示为u在n系中三个转轴方向的投影；n系表示导航坐标系；b系表示载体坐标系；

所述对所述第一姿态矩阵进行微分包括：所述第一姿态矩阵和所述载体坐标系下的旋转角速度值的乘积作为所述微分的结果。

7.根据权利要求6所述的基于惯性导航数据的采煤机自主定位装置，其特征在于，所述第二定位数据包括：采煤机在导航坐标系下的速度值和采煤机在载体坐标系下的位移值。

8.根据权利要求6所述的基于惯性导航数据的采煤机自主定位装置，其特征在于，所述惯性传感仪包括：用于测量所述第一定位数据的平移加速度的摆式加速度计和用于测量所述第一定位数据的旋转角速度值的激光陀螺仪。