[发明专利]一种采煤机机身与摇臂协同定位装置及定位方法

权利要求书

1.一种采煤机机身与摇臂协同定位装置，其特征在于：包括在采煤机机身(5)上安装的机身捷联惯导装置(1)和无线传感锚节点(2)；以及在采煤机割煤一侧的摇臂(6)上安装的摇臂捷联惯导装置(3)和无线传感移动节点(4)；其中无线传感移动节点(4)发射无线信号，无线传感锚节点(2)接收无线信号。

2.根据权利要求1所述的一种采煤机机身与摇臂协同定位装置，其特征在于：所述机身捷联惯导装置(1)包括三只陀螺和三只加速度计。

3.基于权利要求1或2所述的采煤机机身与摇臂协同定位装置的定位方法，具体包括以下步骤：

步骤一、定义坐标系

1)定义采煤机机身坐标系b1：O_b1X_b1Y_b1Z_b1：坐标系原点O_b1固连在机身捷联惯导装置1中心，X_b1轴正向由采煤机指向煤壁，Y_b1轴垂直于X_b1轴向上，Z_b1轴与X_b1轴、Y_b1轴构成右手坐标系，构成前上右坐标系，下标1表示该坐标系为机身坐标系；

2)定义采煤机摇臂坐标系b2：O_b2X_b2Y_b2Z_b2；坐标系原点O_b2固连在摇臂捷联惯导装置中心，X_b2轴正向由摇臂指向煤壁，Y_b2轴垂直于X_b2轴向上，Z_b2轴与X_b2轴、Y_b2轴构成右手坐标系，构成前上右坐标系，下标2表示该坐标系为摇臂坐标系；

3)定义导航坐标系n：O_nX_nY_nZ_n：北天东地理坐标系，X_n轴指向地理北向，Y_n轴指向天向，Z_n轴指向地理东向；

4)导航坐标系n经过三次旋转后与采煤机机身坐标系b1重合，三次旋转的角度即为机身航向角ψ¹、俯仰角θ¹和滚动角γ¹；类似地，导航坐标系n经过三次旋转后与采煤机摇臂坐标系b2重合，三次旋转的角度即为摇臂航向角ψ²、俯仰角θ²和滚动角γ²；

步骤二、计算采煤机机身信息

采煤机机身捷联惯导装置1利用陀螺和加速度计测量信息，解算采煤机机身姿态俯仰角θ¹、航向角ψ¹和滚动角γ¹，计算机身的速度位置和经度λ¹、纬度高度h¹；

1)机身捷联惯导装置1接收初始装订信息：初始位置和初始姿态其中上标1表示机身捷联惯导，N,U,E分别表示导航坐标系下北向、天向和东向位置；同时由于采煤机初始处于停止状态，初始速度V¹＝[0 0 0]^T；

2)机身捷联惯导装置1包括三只陀螺和三只加速度计，三只陀螺测量采煤机机身三个轴的角速度向量三只加速度计测量采煤机机身三个轴向的加速度向量

3)机身捷联惯导装置1首先更新采煤机机身b1坐标系相对于导航坐标系n的角速度向量

其中，表示机身捷联惯导装置1纬度，为机身捷联惯导装置1姿态矩阵，ω_ie是地球自转角速度，R为地球半径，和表示机身捷联惯导装置1东向速度和北向速度；

4)更新四元数q¹＝[q₀ q₁ q₂ q₃]^T

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

5)更新机身捷联惯导装置1相对于导航坐标系的姿态矩阵

其中q¹＝[q₀ q₁ q₂ q₃]^T，令其中i,j＝1,2,3

6)根据计算机身姿态角：

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

航向角

滚动角

7)利用将加速度计输出的加速度向量投影到导航坐标系

8)更新机身捷联惯导装置1的速度位置和经度λ¹、纬度高度h¹，

步骤三、计算采煤机摇臂信息

采煤机摇臂捷联惯导装置3利用陀螺和加速度计测量信息，解算采煤机摇臂姿态俯仰角θ²、航向角ψ²和滚动角γ²，计算摇臂的速度位置和经度λ²、纬度高度h²；

摇臂捷联惯导装置3接收初始装订信息：初始位置和初始姿态其中上标2表示摇臂捷联惯导，N,U,E分别表示导航坐标系下北向、天向和东向位置；同时由于采煤机初始处于停止状态，初始速度V²＝[0 0 0]^T；

摇臂捷联惯导装置3与机身捷联惯导装置1采用相同的位置和姿态解算方法，将步骤二中1)至8)中变量上标1更换为2，即为摇臂捷联惯导装置3的位置和姿态解算方法；

步骤四、计算采煤机机身与摇臂的相对距离

摇臂无线传感移动节点4相对于机身无线传感锚节点2测距：

摇臂无线传感移动节点4发射无线信号，机身无线传感锚节点2接收移动节点的无线信号，利用RSSI算法进行测距，基于RSSI的测距方法将接收到的信号强度转化为无线传感移动节点4与无线传感锚节点2之间的距离，使用公知的对数-常数无线信号传播模型，

S＝A-10mlg(d) (8)

其中，S为信号强度，d为距离，A为接收端距离发射端为1m时接收到的无线信号强度，m为路径损耗，A和m为已知参数，根据S计算距离d；

根据公式(8)，根据无线传感锚节点2接收到的信号强度S，可得摇臂相对于机身的距离d；

在无线传感移动节点4发射无线信号时，摇臂捷联惯导装置3记录该时刻惯导解算的位置信息，并发送给机身捷联惯导装置1；机身无线传感锚节点2接收无线信号时，接收摇臂捷联惯导装置3发送的测距时刻位置信息，同时记录机身捷联惯导装置1解算的机身位置姿态信息及摇臂位置姿态信息；

步骤五、利用步骤四中在无线测距信号发射时刻记录的机身位置姿态信息、摇臂位置姿态信息、机身与摇臂的相对距离，进行基于卡尔曼滤波器的协同导航滤波解算，估计摇臂捷联惯导装置3的位置、速度和姿态误差；

1)协同导航滤波器的状态变量为其中ΔP_I＝[ΔP_N ΔP_U ΔP_E]^T表示摇臂捷联惯导装置3位置误差；ΔV＝[ΔV_N ΔV_U ΔV_E]^T表示摇臂捷联惯导装置3速度误差；φ＝[φ_N φ_U φ_E]^T表示摇臂捷联惯导装置3姿态误差；

2)协同导航滤波状态方程由摇臂捷联惯导装置3的速度误差方程、位置误差方程和姿态误差方程组成：

速度误差方程：

其中，

位置误差方程

姿态误差方程

将方程(9)、(10)、(11)进行离散化并改写为协同导航滤波状态方程形式，其中t_k表示滤波时间点；

X(t_k)＝F(t_k-1)X(t_k-1) (12)

3)协同导航滤波滤波量测方程如下：

4)修正机身捷联惯装置1导同步误差

机身捷联惯导装置1定位数据更新率高于无线测距的定位数据更新率，可采用线性插值方法，在摇臂无线传感移动节点4发射无线信号时，机身捷联惯导装置1记录时间间隔Δt，Δt＜ΔT，采用插值方法，计算用于协同导航的机身捷联惯导装置1的位置P¹：

5)计算量测量Z

在无线测距信号发射时刻，机身无线传感锚节点2位置(即机身捷联惯导装置位置)为在摇臂无线传感移动4节点位置即摇臂捷联惯导装置3位置为根据几何关系有：

其中，Y表示利用摇臂捷联惯导装置3位置与机身捷联惯导装置1位置计算的相对距离。那么协同导航滤波的量测量Z计算如下：

Z＝Y-d

6)机身捷联惯导装置1利用公知的卡尔曼滤波算法，可得协同导航滤波状态量估计值

步骤六、利用协同导航滤波状态估计值修正摇臂捷联惯导装置3的位置、速度和姿态误差，得到误差修正后的摇臂的位置、速度和姿态：

机身捷联惯导装置1将发送给摇臂捷联惯导装置3，对其误差进行闭环点修正：

P²(t_k)＝P²(t_k)-ΔP(t_k) (17)

V²(t_k)＝V²(t_k)-ΔV(t_k) (18)

得到误差修正后的装置3位置、速度和姿态矩阵，对修正误差后的按照步骤二的6)，将变量上标1更换为2，计算误差修正后的摇臂姿态角：俯仰角θ²、航向角ψ²和滚动角γ²。