[发明专利]一种采煤机摇臂惯性/视觉组合定姿装置和方法

权利要求书

1.一种采煤机摇臂惯性/视觉组合定姿方法，其特征在于，该方法通过一种采煤机摇臂惯性/视觉组合定姿装置实现，装置包括目标特征点单元、惯性测量单元、单目视觉测量单元和信息处理单元；

所述的目标特征点单元安装在采煤机摇臂上，惯性测量单元和单目视觉测量单元刚性连接且安装在采煤机上，信息处理单元安装在采煤机电气控制箱内；

单目视觉测量单元用于测量摇臂相对于采煤机的相对姿态，惯性测量单元用于测量采煤机相对于当地水平坐标系的绝对姿态；

所述方法包括以下步骤；

1)以目标特征点单元安装在摇臂上，由4个光学特征点组成，4个特征点共面分布，且呈正方形；

2)单目视觉测量单元利用单个相机拍摄目标特征点单元的图像，利用图像上多个特征点之间的相对位置约束关系，求解特征坐标系与单目视觉测量单元体坐标系之间的姿态关系，并将求解出的姿态传送给信息处理单元；

3)惯性测量单元是一种测量物体三轴角速率和三轴加速度的装置，一般的，惯性测量单元包含了三个单轴的加速度计和三个单轴的陀螺，加速度计测量物体在惯性测量单元体坐标系三个轴向上的加速度信号，陀螺测量惯性测量单元(3)体坐标系相对于当地水平地理坐标系的角速度信号，并将角速度和加速度信号传送给信息处理单元；

4)信息处理单元接收载体三轴角速度和三轴加速度信号，利用捷联惯导算法，解算惯性测量单元相对于当地水平地理坐标系的姿态；

5)信息处理单元根据单目视觉测量单元测量的摇臂相对于采煤机的相对姿态和惯性测量单元测量的采煤机相对于当地水平坐标系的绝对姿态，通过信息融合，确定采煤机摇臂的相对姿态和绝对姿态；

所述的信息处理单元接收惯性测量单元的角速度和加速度信息进行捷联惯导位置和姿态解算；利用采煤机三轴向角度进行采煤机姿态解算，更新姿态四元数和方向余弦矩阵，同时利用当前的当地水平地理坐标系位置，提取采煤机相对于当地水平地理坐标系的姿态；利用姿态方向余弦矩阵，将采煤机三轴向加速度信息投影到当地水平地理坐标系的三个轴向，获得采煤机沿当地水平地理坐标系轴向的加速度，对加速度进行积分，确定采煤机在当地水平地理坐标系上的三维速度和位置；

具体如下：

初始k＝0，信息处理单元接收外部装订信息：经度λ(0)，纬度高度h(0)和初始姿态同时初始速度V＝[0 0 0]^T；

在时刻k，信息处理单元首先更新惯性测量单元体坐标系相对于当地水平地理坐标系的角速度向量

其中，表示纬度值，为姿态矩阵，ω_ie是地球自转角速度，R为地球半径，V_E和V_N表示东向速度和北向速度；

然后，更新四元数q＝[q₀ q₁ q₂ q₃]^T

其中，ΔT表示姿态计算周期；

然后，更新惯性测量单元3相对于当地水平地理坐标系的惯性姿态矩阵

令其中i,j＝1,2,3

根据采煤机坐标系相对于当地水平地理坐标系的姿态角计算如下：

俯仰角θ_b＝sin^-1(C₁₂)

航向角

滚动角

利用将加速度计输出的加速度向量投影到当地水平地理坐标系fⁿ＝[f_N f_U f_E]^T，

然后，更新惯性测量单元3的速度V＝[V_N V_U V_E]^T，经度λ，纬度和高度h；

h(k+1)＝h(k)+0.5ΔT(V_U(k)+V_U(k+1))

令k←k+1，继续下一个时刻的计算。