CN111323011A - Cooperative positioning device and positioning method of shearer body and rocker arm - Google Patents

Abstract

A co-location device and a co-location method for a coal mining machine body and a rocker arm comprise a body strapdown inertial navigation device and a wireless sensing anchor node which are arranged on the coal mining machine body; the rocker arm strapdown inertial navigation device and the wireless sensing mobile node are arranged on the rocker arm on the coal cutting side of the coal cutter; the wireless sensing anchor node receives the wireless signal; during work, the machine body moves along the track, the rocker arm cuts a coal wall to move, the rocker arm strapdown inertial navigation device measures angular velocity and acceleration information of the rocker arm of the coal mining machine, the machine body strapdown inertial navigation device measures the angular velocity and acceleration information of the machine body of the coal mining machine in real time, and the strapdown navigation algorithm is used for positioning and attitude determination; the invention has the advantages of high positioning and attitude determination precision, high fully mechanized mining efficiency and remote automatic control.

Description

Cooperative positioning device and positioning method of shearer body and rocker arm

technical field

The invention belongs to the technical field of shearer positioning, and in particular relates to a cooperative positioning device and a positioning method of a shearer body and a rocker arm.

Background technique

Shearer, hydraulic support and scraper conveyor are the three most important equipments in underground fully mechanized mining face, which cooperate with each other to complete coal cutting, coal transportation and support work. Among them, the shearer is the main equipment, which is the main equipment for coal cutting and coal loading in the fully mechanized mining face, and is a highly integrated fully mechanized mining equipment. During operation, the shearer performs reciprocating coal cutting along the track of the scraper conveyor, and the hydraulic support supports the roof and pushes the working face. In order to realize the automation and remote automatic control of the fully mechanized mining face, it is necessary to precisely and dynamically position the shearer. Moreover, for the needs of automatic coal mining, the fuselage and rocker arm of the shearer need to be positioned at the same time. And the positioning technology of rocker arm is the key technology of coal mine production equipment automation.

The working conditions of the fully mechanized coal mining face are complex and the space is closed, so the positioning of the shearer body and rocker arm is a typical indoor positioning problem in a complex closed environment. In the coal mining operation of the shearer, the body of the shearer moves along the track; while the rocker arm performs the coal cutting work, there is a relative movement relative to the body of the shearer. In order to realize automatic coal mining, the position and attitude of the shearer body and rocker arm must be measured at the same time.

At present, the positioning methods used by shearers mainly include strapdown inertial navigation positioning method, infrared positioning method, ultrasonic positioning method, gear counting positioning method, wireless sensor network positioning method and so on.

In the infrared positioning method, the infrared transmitting device installed on the shearer transmits the signal, the receiving device installed on the hydraulic support receives the signal, and uses the infrared ranging to locate the position of the shearer, but the infrared signal is easily affected by dust, and there is an inherent positioning blind spot. , the positioning accuracy is not high, so the use is limited.

The ultrasonic positioning method installs the ultrasonic transmitting device in the roadway of the working face. When the shearer passes by, the fuselage emits ultrasonic waves, and the ultrasonic receiving device receives the signal according to each position, and uses the ultrasonic ranging to locate the position of the shearer. The advantages of ultrasonic waves are: It can penetrate dust, but due to the long working surface, serious signal loss, and low positioning accuracy, there are limitations in its use.

The gear counting and positioning method is to count the number of rotations of the walking gear of the shearer, and calculate the displacement of the shearer along the conveyor track according to the number of rotations and the circumference of the gear; but this method can only be used to locate the shearer The one-dimensional position along the track direction, and affected by the gear count error, cannot meet the needs of three-dimensional positioning.

The wireless sensor network positioning method is to arrange multiple wireless sensors (called anchor nodes) with known positions on the hydraulic support, and arrange the nodes to be located (called mobile nodes) on the shearer, the mobile nodes transmit wireless signals, and the anchor nodes receive The wireless signal monitors the positional relationship between the shearer and the hydraulic support, and calculates the position of the shearer; however, due to the complex working face environment, the wireless positioning data is unstable, and the anchor node will change its position after moving with the hydraulic support. The anchor node position information is updated, and at the same time, the shearer attitude cannot be determined, which cannot meet the needs of real-time positioning and attitude determination.

The strapdown inertial navigation positioning method is a fully autonomous navigation and positioning method, without external information, using the three-axis gyroscope and three-axis accelerometer of the strapdown inertial navigation device to measure the angular velocity and linear acceleration of the shearer in real time, combined with the initial binding information , through the attitude update algorithm, the motion attitude of the shearer is first solved, and then the acceleration is projected to the navigation coordinate system according to the attitude information, and the speed and position of the shearer are obtained through integration and quadratic integration. Accuracy requirements, but due to volume, weight and other reasons, it is generally only installed on the fuselage of the shearer, and cannot be used for the positioning and attitude determination of the shearer rocker arm, or the shearer rocker arm can only be installed with small and low-precision inertial navigation. , its positioning and attitude accuracy cannot meet the requirements.

SUMMARY OF THE INVENTION

In order to overcome the above-mentioned problems in the prior art, the present invention provides a cooperative positioning device and a positioning method of the shearer body and the rocker arm, which can utilize the strapdown inertial navigation device and wireless The distance measuring sensor can realize the positioning and attitude of the shearer body and the rocker arm at the same time, and has the advantages of high positioning and attitude accuracy, high working efficiency of fully mechanized mining and remote automatic control.

In order to achieve the above object, the present invention adopts the following technical scheme:

A cooperative positioning device for a shearer body and a rocker arm, comprising a body strapdown inertial navigation device 1 and a wireless sensor anchor node 2 installed on the shearer body 5; and a coal cutting side of the shearer The rocker arm strapdown inertial navigation device 3 and the wireless sensor mobile node 4 are installed on the rocker arm 6; wherein the wireless sensor mobile node 4 transmits wireless signals, and the wireless sensor anchor node 2 receives wireless signals.

The fuselage strapdown inertial navigation device 1 includes three gyroscopes and three accelerometers.

Based on the above-mentioned positioning method of the shearer fuselage and the rocker arm cooperative positioning device, it specifically includes the following steps:

Step 1. Define the coordinate system

1) Define the shearer fuselage coordinate system b1: O _b1 X _b1 Y _b1 Z _b1 : the origin of the coordinate system O _b1 is fixedly connected to the center of the fuselage strapdown inertial navigation device 1, and the positive X _b1 axis points from the shearer to the coal wall, the Y _b1 axis is perpendicular to the X _b1 axis upward, the Z _b1 axis, the X _b1 axis, and the Y _b1 axis form a right-hand coordinate system, which constitutes the front upper right coordinate system, and the subscript 1 indicates that the coordinate system is the fuselage coordinate system;

2) Define the shearer rocker arm coordinate system b2: O _b2 X _b2 Y _b2 Z _b2 ; the origin of the coordinate system O _b2 is fixedly connected to the center of the rocker arm strapdown inertial navigation device 3, and the X _b2 axis is directed to the coal wall by the rocker arm. , the Y _b2 axis is perpendicular to the X _b2 axis upward, the Z _b2 axis, the X _b2 axis, and the Y _b2 axis form a right-hand coordinate system, forming a front upper right coordinate system, and the subscript 2 indicates that the coordinate system is a rocker arm coordinate system;

3) Define the navigation coordinate system n: O _n X _n Y _n Z _n : the north celestial east geographic coordinate system, the X _n axis points to the geographic north direction, the Y _n axis points to the sky direction, and the Z _n axis points to the geographic east direction;

4) The navigation coordinate system n coincides with the shearer fuselage coordinate system b1 after three rotations, and the angles of the three rotations are the fuselage heading angle ψ ¹ , pitch angle θ ¹ and roll angle γ ¹ ; similarly, the navigation coordinate system After three rotations, n coincides with the shearer rocker arm coordinate system b2, and the angles of the three rotations are rocker arm heading angle ψ ² , pitch angle θ ² and roll angle γ ² ;

Step 2. Calculate the information of the shearer fuselage

位置

和经度λ¹、纬度

高度h¹；The shearer fuselage strapdown inertial navigation device 1 uses the gyro and accelerometer measurement information to calculate the shearer fuselage attitude pitch angle θ ¹ , heading angle ψ ¹ and roll angle γ ¹ , and calculate the speed of the body

Location

and longitude λ ¹ , latitude

height h ¹ ;

和初始姿态

其中上标1表示机身捷联惯导，N,U,E分别表示导航坐标系下北向、天向和东向位置；同时由于采煤机初始处于停止状态，初始速度V¹＝[0 0 0]^T；1) Body SINS 1 receives initial binding information: initial position

and initial posture

The superscript ¹ represents the fuselage strapdown inertial navigation, and N, U, and E represent the north, sky and east positions in the navigation coordinate system, respectively. 0] ^T ;

三只加速度计测量采煤机机身三个轴向的加速度向量

2) The three gyroscopes of the fuselage strapdown inertial navigation device 1 measure the angular velocity vectors of the three axes of the shearer fuselage

Three accelerometers measure the acceleration vectors in three axial directions of the shearer fuselage

3) The fuselage strapdown inertial navigation device 1 first updates the angular velocity vector of the shearer fuselage b1 coordinate system relative to the navigation coordinate system n

表示机身捷联惯导装置1纬度，

为机身捷联惯导装置1姿态矩阵，ω_ie是地球自转角速度，R为地球半径，

和

表示机身捷联惯导装置1东向速度和北向速度；in,

Indicates the 1 latitude of the fuselage strapdown inertial navigation device,

is the attitude matrix of the fuselage SINS1, ω _ie is the angular velocity of the earth's rotation, R is the earth's radius,

and

Indicates the east speed and north speed of the fuselage strapdown inertial navigation device 1;

4) Update quaternion q ¹ =[q ₀ q ₁ q ₂ q ₃ ] ^T

T表示姿态计算周期；in,

T represents the attitude calculation period;

5) Update the attitude matrix of the fuselage strapdown inertial navigation device 1 relative to the navigation coordinate system

其中i,j＝1,2,3where q ¹ =[q ₀ q ₁ q ₂ q ₃ ] ^T , let

where i,j=1,2,3

计算机身姿态角：6) According to

Computer body attitude angle:

Pitch angle θ ¹ =sin ⁻¹ (C ₁₂ )

Heading

roll angle

将加速度计输出的加速度向量

投影到导航坐标系

7) Utilize

Convert the acceleration vector output by the accelerometer

Project to Navigation Coordinate System

位置

和经度λ¹、纬度

高度h¹，8) Update the speed of the fuselage SINS 1

Location

and longitude λ ¹ , latitude

height h ¹ ,

Step 3. Calculate the rocker arm information of the shearer

位置

和经度λ²、纬度

高度h²；The shearer rocker arm strapdown inertial navigation device 3 uses the gyro and accelerometer measurement information to solve the shearer rocker arm attitude pitch angle θ ² , heading angle ψ ² and roll angle γ ² , and calculate the rocker arm speed

Location

and longitude λ ² , latitude

height h ² ;

和初始姿态

其中上标2表示摇臂捷联惯导，N,U,E分别表示导航坐标系下北向、天向和东向位置；同时由于采煤机初始处于停止状态，初始速度V²＝[0 0 0]^T；Rocker arm SINS 3 receives initial binding information: initial position

and initial posture

The superscript ² represents the rocker arm strapdown inertial navigation, and N, U, and E represent the north, sky and east positions in the navigation coordinate system, respectively. 0] ^T ;

The rocker arm strapdown inertial navigation device 3 and the fuselage strapdown inertial navigation device 1 adopt the same position and attitude calculation method, and the variable superscript 1 in step 2 in 1) to 8) is replaced by 2, that is, the rocker arm The position and attitude calculation method of the coupled inertial navigation device 3;

Step 4. Calculate the relative distance between the shearer body and the rocker arm

The rocker arm wireless sensor mobile node 4 measures the distance relative to the fuselage wireless sensor anchor node 2:

The rocker arm wireless sensor mobile node 4 transmits wireless signals, the fuselage wireless sensor anchor node 2 receives the wireless signal of the mobile node, and uses the RSSI algorithm for ranging, and the RSSI-based ranging method converts the received signal strength into wireless transmission. sense the distance between the mobile node 4 and the wireless sensing anchor node 2, using the well-known log-constant wireless signal propagation model,

S=A-10m lg(d) (8)

Among them, S is the signal strength, d is the distance, A is the received wireless signal strength when the receiving end is 1m away from the transmitting end, m is the path loss, A and m are known parameters, and the distance d is calculated according to S;

According to formula (8), according to the signal strength S received by the wireless sensor anchor node 2, the distance d of the rocker arm relative to the fuselage can be obtained;

When the wireless sensor mobile node 4 transmits a wireless signal, the rocker arm SINS device 3 records the position information calculated by the inertial navigation at that moment, and sends it to the fuselage SINS device 1; the fuselage wireless sensor anchor node 2. When receiving wireless signals, receive the ranging moment position information sent by the rocker arm strapdown inertial navigation device 3, and simultaneously record the body position and attitude information and the rocker arm position and attitude information calculated by the fuselage strapdown inertial navigation device 1.

Step 5: Use the fuselage position and attitude information, the rocker arm position and attitude information, and the relative distance between the fuselage and the rocker arm recorded at the time of transmitting the wireless ranging signal in step 4, to carry out the Kalman filter-based collaborative navigation filtering solution, Estimate the position, velocity and attitude errors of the rocker arm SINS 3;

其中

表示摇臂捷联惯导装置3位置误差；

表示摇臂捷联惯导装置3速度误差；φ＝[φ_N φ_U φ_E]^T表示摇臂捷联惯导装置3姿态误差；1) The state variable of the collaborative navigation filter is

in

Indicates the position error of the rocker arm strapdown inertial navigation device 3;

Represents the speed error of the rocker arm SINS device 3; φ=[φ _N φ _U φ _E ] ^T represents the attitude error of the rocker arm SINS device 3;

2) The cooperative navigation filtering state equation is composed of the velocity error equation, position error equation and attitude error equation of the rocker arm SINS device 3:

Velocity Error Equation:

in,

Position Error Equation

Attitude Error Equation

The equations (9), (10) and (11) are discretized and rewritten into the form of cooperative navigation filtering state equation, where t _k represents the filtering time point;

X(t _k )=F(t _k-1 )X(t _k-1 ) (12)

3) The collaborative navigation filtering measurement equation is as follows:

4) Correct the 1-lead synchronization error of the fuselage strapdown inertial device

The update rate of the positioning data of the fuselage strapdown inertial navigation device 1 is higher than that of the wireless ranging, and the linear interpolation method can be used. When the rocker arm wireless sensor mobile node 4 transmits wireless signals, the fuselage strapdown inertial navigation 1. Record the time interval Δt, Δt<ΔT, and use the interpolation method to calculate the position P ¹ of the fuselage strapdown inertial navigation device 1 used for cooperative navigation:

5) Calculate the amount of measurement Z

在摇臂无线传感移动节点4位置即摇臂捷联惯导装置3位置为

根据几何关系有：At the moment when the wireless ranging signal is transmitted, the position of the fuselage wireless sensor anchor node 2 (that is, the position of the fuselage strapdown inertial navigation device) is

At the position of the rocker arm wireless sensor mobile node 4, that is, the position of the rocker arm strapdown inertial navigation device 3 is:

According to the geometric relationship there are:

Among them, Y represents the relative distance calculated by using the position of the rocker arm SINS device 3 and the position of the fuselage SINS device 1 . Then the quantity measurement Z of the collaborative navigation filtering is calculated as follows:

Z=Y-d

6) The fuselage strapdown inertial navigation device 1 uses the well-known Kalman filter algorithm to obtain the estimated value of the cooperative navigation filter state quantity

修正摇臂捷联惯导装置3的位置、速度和姿态误差，得到误差修正后的摇臂的位置、速度和姿态：Step 6. Use collaborative navigation to filter state estimates

Correct the position, speed and attitude errors of the rocker arm strapdown inertial navigation device 3, and obtain the position, speed and attitude of the rocker arm after error correction:

发送给摇臂捷联惯导装置3，对其误差进行闭环点修正：The fuselage strapdown inertial navigation device 1 will be

Send it to the rocker arm strapdown inertial navigation device 3, and perform closed-loop point correction for its error:

P ² (t _k )=P ² (t _k )-ΔP(t _k ) (17)

V ² (t _k )=V ² (t _k )-ΔV(t _k ) (18)

按照步骤二的6)，将变量上标1更换为2，计算误差修正后的摇臂姿态角：俯仰角θ²、航向角ψ²和滚动角γ²。The position, velocity and attitude matrix of rocker arm SINS device 3 after error correction is obtained.

According to 6) of step 2, replace the variable superscript 1 with 2, and calculate the attitude angle of the rocker arm after error correction: pitch angle θ ² , heading angle ψ ² and roll angle γ ² .

The beneficial effects of the present invention are as follows: a method for co-locating the fuselage 5 and the rocker arm 6 of the shearer is provided; The device 3 calculates the position and attitude of the rocker arm, and estimates and corrects the position and attitude errors of the rocker arm through the wireless ranging between the fuselage wireless sensor anchor node 2 and the rocker arm wireless sensor mobile node 4, using the collaborative navigation filtering method , to achieve high-precision positioning and attitude determination of the shearer fuselage 5 and rocker arm 6, so that it can meet the needs of automatic and remote automatic control of fully mechanized mining face, with high positioning and attitude accuracy, high fully mechanized mining work efficiency and remote automatic control. Advantages of control.

Description of drawings

Figure 1 is a schematic diagram of the cooperative positioning of the shearer body and the rocker arm.

Figure 2 is a schematic diagram of the fuselage (rocker) coordinate system, the navigation coordinate system and the attitude.

Figure 3 is a flow chart of the co-location of the shearer body and the rocker arm.

Figure 4 is a time sequence diagram of shearer inertial/wireless sensor network combined positioning information.

In the figure: 1. Airframe strapdown inertial navigation device; 2. Airframe wireless sensor anchor node; 3. Rocker arm strapdown inertial navigation device; 4. Rocker arm wireless sensor mobile node; 5. Airframe; 6. rocker arm.

Detailed ways

The present invention will be further described in detail below in conjunction with the accompanying drawings.

Referring to Fig. 1, a shearer fuselage and rocker arm co-positioning device, comprising a fuselage strapdown inertial navigation device 1 and a wireless sensor anchor node 2 installed on the shearer fuselage; The rocker arm strapdown inertial navigation device 3 and the wireless sensor mobile node 4 are installed on the rocker arm 6 on the coal side; the wireless sensor mobile node 4 transmits wireless signals, and the wireless sensor anchor node 2 receives wireless signals.

The fuselage strapdown inertial navigation device 1 includes three gyroscopes and three accelerometers.

Based on the above-mentioned positioning method of the shearer fuselage and the rocker arm cooperative positioning device, the specific implementation is as follows:

Step 1. Define the coordinate system

1) Define the shearer fuselage coordinate system b1: O _b1 X _b1 Y _b1 Z _b1 : the origin of the coordinate system O _b1 is fixedly connected to the center of the fuselage strapdown inertial navigation device 1, and the positive X _b1 axis points from the shearer to the coal wall, the Y _b1 axis is perpendicular to the X _b1 axis upward, the Z _b1 axis, the X _b1 axis, and the Y _b1 axis form a right-hand coordinate system, which constitutes the front upper right coordinate system, and the subscript 1 indicates that the coordinate system is the fuselage coordinate system;

2) Define the shearer rocker arm coordinate system b2: O _b2 X _b2 Y _b2 Z _b2 ; the origin of the coordinate system O _b2 is fixedly connected to the center of the rocker arm strapdown inertial navigation device, and the positive X _b2 axis is directed by the rocker arm to the coal wall, The Y _b2 axis is perpendicular to the X _b2 axis upward, the Z _b2 axis, the X _b2 axis, and the Y _b2 axis form a right-hand coordinate system, which constitutes the front upper right coordinate system, and the subscript 2 indicates that the coordinate system is a rocker arm coordinate system;

3) Define the navigation coordinate system n: O _n X _n Y _n Z _n : the north celestial east geographic coordinate system, the X _n axis points to the geographic north direction, the Y _n axis points to the sky direction, and the Z _n axis points to the geographic east direction;

4) The navigation coordinate system n coincides with the shearer fuselage coordinate system b1 after three rotations, and the angles of the three rotations are the fuselage heading angle ψ ¹ , pitch angle θ ¹ and roll angle γ ¹ , as shown in Figure 2; Similarly, the navigation coordinate system n coincides with the shearer rocker arm coordinate system b2 after three rotations, and the angles of the three rotations are the rocker arm heading angle ψ ² , pitch angle θ ² and roll angle γ ² ;

Step 2. Calculate the information of the shearer fuselage

位置

和经度λ¹、纬度

高度h¹；The shearer fuselage strapdown inertial navigation device 1 uses the gyro and accelerometer measurement information to calculate the shearer fuselage attitude pitch angle θ ¹ , heading angle ψ ¹ and roll angle γ ¹ , and calculate the speed of the body

Location

and longitude λ ¹ , latitude

height h ¹ ;

和初始姿态

其中上标1表示机身捷联惯导，N,U,E分别表示导航坐标系下北向、天向和东向位置；同时由于采煤机初始处于停止状态，初始速度V¹＝[0 0 0]^T；1) Body SINS 1 receives initial binding information: initial position

and initial posture

The superscript ¹ represents the fuselage strapdown inertial navigation, and N, U, and E represent the north, sky and east positions in the navigation coordinate system, respectively. 0] ^T ;

三只加速度计测量采煤机机身三个轴向的加速度向量

2) The three gyroscopes of the fuselage strapdown inertial navigation device 1 measure the angular velocity vectors of the three axes of the shearer fuselage

Three accelerometers measure the acceleration vectors in three axial directions of the shearer fuselage

3) The fuselage strapdown inertial navigation device 1 first updates the angular velocity vector of the shearer fuselage b1 coordinate system relative to the navigation coordinate system n

表示机身捷联惯导装置1纬度，

为机身捷联惯导装置1姿态矩阵，ω_ie是地球自转角速度，R为地球半径，

和

表示机身捷联惯导装置1东向速度和北向速度；in,

Indicates the 1 latitude of the fuselage strapdown inertial navigation device,

is the attitude matrix of the fuselage SINS1, ω _ie is the angular velocity of the earth's rotation, R is the earth's radius,

and

Indicates the east speed and north speed of the fuselage strapdown inertial navigation device 1;

4) Update quaternion q ¹ =[q ₀ q ₁ q ₂ q ₃ ] ^T

T表示姿态计算周期；in,

T represents the attitude calculation period;

5) Update the attitude matrix of the fuselage strapdown inertial navigation device 1 relative to the navigation coordinate system

其中i,j＝1,2,3where q ¹ =[q ₀ q ₁ q ₂ q ₃ ] ^T , let

where i,j=1,2,3

计算机身姿态角：6) According to

Computer body attitude angle:

Pitch angle θ ¹ =sin ⁻¹ (C ₁₂ )

Heading

roll angle

将加速度计输出的加速度向量

投影到导航坐标系

7) Utilize

Convert the acceleration vector output by the accelerometer

Project to Navigation Coordinate System

位置

和经度λ¹、纬度

高度h¹，8) Update the speed of the fuselage SINS 1

Location

and longitude λ ¹ , latitude

height h ¹ ,

Step 3. Calculate the rocker arm information of the shearer

位置

和经度λ²、纬度

高度h²；The shearer rocker arm strapdown inertial navigation device 3 uses the gyro and accelerometer measurement information to solve the shearer rocker arm attitude pitch angle θ ² , heading angle ψ ² and roll angle γ ² , and calculate the rocker arm speed

Location

and longitude λ ² , latitude

height h ² ;

和初始姿态

其中上标2表示摇臂捷联惯导，N,U,E分别表示导航坐标系下北向、天向和东向位置；同时由于采煤机初始处于停止状态，初始速度V²＝[0 0 0]^T；Rocker arm SINS 3 receives initial binding information: initial position

and initial posture

The superscript ² represents the rocker arm strapdown inertial navigation, and N, U, and E represent the north, sky and east positions in the navigation coordinate system, respectively. 0] ^T ;

The rocker arm strapdown inertial navigation device 3 and the fuselage strapdown inertial navigation device 1 adopt the same position and attitude calculation method, and the variable superscript 1 in step 2 in 1) to 8) is replaced by 2, that is, the rocker arm The position and attitude calculation method of the coupled inertial navigation device 3;

Step 4. Calculate the relative distance between the shearer body and the rocker arm

The rocker arm wireless sensor mobile node 4 measures the distance relative to the fuselage wireless sensor anchor node 2:

The rocker arm wireless sensor mobile node 4 transmits wireless signals, the fuselage wireless sensor anchor node 2 receives the wireless signal of the mobile node, and uses the RSSI algorithm for ranging, and the RSSI-based ranging method converts the received signal strength into wireless transmission. sense the distance between the mobile node 4 and the wireless sensing anchor node 2, using the well-known log-constant wireless signal propagation model,

S=A-10m lg(d) (8)

Among them, S is the signal strength, d is the distance, A is the received wireless signal strength when the receiving end is 1m away from the transmitting end, m is the path loss, A and m are known parameters, and the distance d is calculated according to S;

According to formula (8), according to the signal strength S received by the wireless sensor anchor node 2, the distance d of the rocker arm relative to the fuselage can be obtained;

When the wireless sensor mobile node 4 transmits a wireless signal, the rocker arm SINS device 3 records the position information calculated by the inertial navigation at that moment, and sends it to the fuselage SINS device 1; the fuselage wireless sensor anchor node 2. When receiving the wireless signal, receive the position information at the time of ranging sent by the rocker arm strapdown inertial navigation device 3, and simultaneously record the body position and attitude information and the rocker arm position and attitude information calculated by the fuselage strapdown inertial navigation device 1;

Step 5: Use the fuselage position and attitude information, the rocker arm position and attitude information, and the relative distance between the fuselage and the rocker arm recorded at the time of transmitting the wireless ranging signal in step 4, to carry out the Kalman filter-based collaborative navigation filtering solution, Estimate the position, velocity and attitude errors of the rocker arm SINS 3;

其中ΔP_I＝[ΔP_N ΔP_UΔP_E]^T表示摇臂捷联惯导装置3位置误差；ΔV＝[ΔV_N ΔV_U ΔV_E]^T表示摇臂捷联惯导装置3速度误差；φ＝[φ_N φ_U φ_E]^T表示摇臂捷联惯导装置3姿态误差；1) The state variable of the collaborative navigation filter is

Wherein ΔP _I =[ΔP _N ΔP _U ΔP _E ] ^T represents the position error of the rocker arm SINS 3; ΔV=[ΔV _N ΔV _U ΔV _E ] ^T represents the speed error of the rocker arm SINS 3; φ= [φ _N φ _U φ _E ] ^T represents the attitude error of the rocker arm strapdown inertial navigation device 3;

2) The cooperative navigation filtering state equation is composed of the velocity error equation, position error equation and attitude error equation of the rocker arm SINS device 3:

Velocity Error Equation:

in,

Position Error Equation

Attitude Error Equation

The equations (9), (10) and (11) are discretized and rewritten into the form of cooperative navigation filtering state equation, where t _k represents the filtering time point;

X(t _k )=F(t _k-1 )X(t _k-1 ) (12)

3) The collaborative navigation filtering filtering measurement equation is as follows:

4) Correct the 1-lead synchronization error of the fuselage strapdown inertial device

The update rate of the positioning data of the fuselage strapdown inertial navigation device 1 is higher than the update rate of the positioning data of the wireless ranging, and the interpolation method shown in Figure 4 is used. The strap-down inertial navigation device 1 records the time interval Δt, Δt<ΔT, and uses the interpolation method to calculate the position P ¹ of the fuselage strap-down inertial navigation device 1 for cooperative navigation:

5) Calculate the amount of measurement Z

在摇臂无线传感移动节点4位置即摇臂捷联惯导装置3位置为

根据几何关系有：At the moment when the wireless ranging signal is transmitted, the position of the fuselage wireless sensor anchor node 2 (that is, the position of the fuselage strapdown inertial navigation device) is

At the position of the rocker arm wireless sensor mobile node 4, that is, the position of the rocker arm strapdown inertial navigation device 3 is:

According to the geometric relationship there are:

Among them, Y represents the relative distance calculated by using the position of the rocker arm SINS device 3 and the position of the fuselage SINS device 1 . Then the quantity measurement Z of the collaborative navigation filtering is calculated as follows:

Z=Y-d

6) The fuselage strapdown inertial navigation device 1 uses the well-known Kalman filter algorithm to obtain the estimated value of the cooperative navigation filter state quantity

修正摇臂捷联惯导装置3的位置、速度和姿态误差，得到误差修正后的摇臂的位置、速度和姿态：Step 6. Use collaborative navigation to filter state estimates

Correct the position, speed and attitude errors of the rocker arm strapdown inertial navigation device 3, and obtain the position, speed and attitude of the rocker arm after error correction:

发送给摇臂捷联惯导装置3，对其误差进行闭环点修正：The fuselage strapdown inertial navigation device 1 will be

Send it to the rocker arm strapdown inertial navigation device 3, and perform closed-loop point correction for its error:

P ² (t _k )=P ² (t _k )-ΔP(t _k ) (17)

V ² (t _k )=V ² (t _k )-ΔV(t _k ) (18)

按照步骤二的6)，将变量上标1更换为2，计算误差修正后的摇臂姿态角：俯仰角θ²、航向角ψ²和滚动角γ²。The position, velocity and attitude matrix of rocker arm SINS device 3 after error correction is obtained.

According to 6) of step 2, replace the variable superscript 1 with 2, and calculate the attitude angle of the rocker arm after error correction: pitch angle θ ² , heading angle ψ ² and roll angle γ ² .

Referring to Figure 3, the working principle of the present invention is:

In the coal mining operation of the shearer, the shearer body 5 moves along the track, while the rocker arm 6 moves to cut the coal wall; the rocker arm strapdown inertial navigation device 3 measures the angular velocity and acceleration information of the shearer rocker arm , using the strapdown navigation algorithm for positioning and attitude determination; the rocker arm wireless sensor mobile node 4 transmits wireless signals, and the rocker arm strapdown inertial navigation device 3 sends the inertial information at this moment to the fuselage strapdown inertial navigation device 1; The strapdown inertial navigation device 1 measures the angular velocity and acceleration information of the shearer fuselage in real time, and uses the strapdown navigation algorithm for positioning and attitude determination; the fuselage wireless sensor anchor node 2 receives the wireless signal of the rocker arm wireless sensor mobile node 4 , using the RSSI algorithm to calculate the distance; the fuselage SINS 1 calculates the inertial information of the fuselage at the time of arrival of the wireless signal, receives the inertial information of the rocker SINS 3, and communicates with the fuselage wireless sensor anchor The node 2 ranging and positioning results are subjected to collaborative navigation filtering to estimate the position and attitude error of the rocker arm; according to the error estimation results, the corrected rocker arm position and attitude information are sent to the rocker arm strapdown inertial navigation device, and the positioning and attitude of the rocker arm are corrected. The attitude error is determined, so as to obtain the precise position and attitude information of the shearer fuselage and rocker arm at the same time.

It should be pointed out that although the present invention takes the strapdown inertial navigation device as a specific implementation example, the present invention only needs to slightly change the navigation solution method in steps 1 to 6, and is also applicable to platform inertial navigation and other forms of inertial navigation. device.

Obviously, the embodiments in the above specific embodiments are only used to illustrate the technical solutions of the present invention and not to limit them. Although the present invention has been described in detail with reference to the above specific embodiments, those of ordinary skill in the art should understand that: the present invention can still be The specific embodiments of the present invention are modified or equivalently replaced, and any modification or equivalent replacement that does not depart from the spirit and scope of the present invention shall be included in the scope of the present claims.

Claims (3)

1. A shearer fuselage and a rocker arm cooperative positioning device, characterized in that: comprising the fuselage strapdown inertial navigation device (1) and the wireless sensor anchor node (1) installed on the shearer fuselage (5). 2); and a rocker arm strapdown inertial navigation device (3) and a wireless sensor mobile node (4) installed on the rocker arm (6) on the coal cutting side of the shearer; wherein the wireless sensor mobile node (4) The wireless signal is transmitted, and the wireless sensor anchor node (2) receives the wireless signal. 2. A shearer body and rocker arm cooperative positioning device according to claim 1, characterized in that: the body strapdown inertial navigation device (1) comprises three gyroscopes and three accelerometers. 3. Based on the positioning method of the shearer fuselage and the rocker arm cooperative positioning device according to claim 1 or 2, the method specifically comprises the following steps: Step 1. Define the coordinate system 1) Define the shearer fuselage coordinate system b1: O _b1 X _b1 Y _b1 Z _b1 : the origin of the coordinate system O _b1 is fixedly connected to the center of the fuselage strapdown inertial navigation device 1, and the positive X _b1 axis points from the shearer to the coal wall, the Y _b1 axis is perpendicular to the X _b1 axis upward, the Z _b1 axis, the X _b1 axis, and the Y _b1 axis form a right-hand coordinate system, which constitutes the front upper right coordinate system, and the subscript 1 indicates that the coordinate system is the fuselage coordinate system; 2) Define the shearer rocker arm coordinate system b2: O _b2 X _b2 Y _b2 Z _b2 ; the origin of the coordinate system O _b2 is fixedly connected to the center of the rocker arm strapdown inertial navigation device, and the positive X _b2 axis is directed by the rocker arm to the coal wall, The Y _b2 axis is perpendicular to the X _b2 axis upward, and the Z _b2 axis, the X _b2 axis, and the Y _b2 axis form a right-hand coordinate system, forming the front upper right coordinate system, and the subscript 2 indicates that the coordinate system is a rocker arm coordinate system; 3) Define the navigation coordinate system n: O _n X _n Y _n Z _n : the north celestial east geographic coordinate system, the X _n axis points to the geographic north direction, the Y _n axis points to the sky direction, and the Z _n axis points to the geographic east direction; 4) The navigation coordinate system n coincides with the shearer fuselage coordinate system b1 after three rotations, and the angles of the three rotations are the fuselage heading angle ψ ¹ , pitch angle θ ¹ and roll angle γ ¹ ; similarly, the navigation coordinate system After three rotations, n coincides with the shearer rocker arm coordinate system b2, and the angles of the three rotations are rocker arm heading angle ψ ² , pitch angle θ ² and roll angle γ ² ; Step 2. Calculate the information of the shearer fuselage The shearer fuselage strapdown inertial navigation device 1 uses the gyro and accelerometer measurement information to calculate the shearer fuselage attitude pitch angle θ ¹ , heading angle ψ ¹ and roll angle γ ¹ , and calculate the speed of the body

Location

and longitude λ ¹ , latitude

height h ¹ ; 1) Body SINS 1 receives initial binding information: initial position

and initial posture

The superscript ¹ represents the fuselage strapdown inertial navigation, and N, U, and E represent the north, sky and east positions in the navigation coordinate system respectively. 0] ^T ; 2) The fuselage strapdown inertial navigation device 1 includes three gyroscopes and three accelerometers, and the three gyroscopes measure the angular velocity vectors of the three axes of the shearer fuselage

Three accelerometers measure the acceleration vectors in three axial directions of the shearer fuselage

3) The fuselage strapdown inertial navigation device 1 first updates the angular velocity vector of the shearer fuselage b1 coordinate system relative to the navigation coordinate system n

in,

Indicates the 1 latitude of the fuselage strapdown inertial navigation device,

is the attitude matrix of the fuselage SINS1, ω _ie is the angular velocity of the earth's rotation, R is the earth's radius,

and

Indicates the east speed and north speed of the fuselage strapdown inertial navigation device 1; 4) Update quaternion q ¹ =[q ₀ q ₁ q ₂ q ₃ ] ^T

in,

T represents the attitude calculation period; 5) Update the attitude matrix of the fuselage strapdown inertial navigation device 1 relative to the navigation coordinate system

where q ¹ =[q ₀ q ₁ q ₂ q ₃ ] ^T , let

where i,j=1,2,3 6) According to

Computer body attitude angle: Pitch angle θ ¹ =sin ⁻¹ (C ₁₂ ) Heading

roll angle

7) Utilize

Convert the acceleration vector output by the accelerometer

Project to Navigation Coordinate System

8) Update the speed of the fuselage SINS 1

Location

and longitude λ ¹ , latitude

height h ¹ ,

Step 3. Calculate the rocker arm information of the shearer The shearer rocker arm strapdown inertial navigation device 3 uses the gyro and accelerometer measurement information to solve the shearer rocker arm attitude pitch angle θ ² , heading angle ψ ² and roll angle γ ² , and calculate the rocker arm speed

Location

and longitude λ ² , latitude

height h ² ; Rocker arm SINS 3 receives initial binding information: initial position

and initial posture

The superscript ² represents the rocker arm strapdown inertial navigation, and N, U, and E represent the north, sky and east positions in the navigation coordinate system, respectively. 0] ^T ; The rocker arm strapdown inertial navigation device 3 and the fuselage strapdown inertial navigation device 1 adopt the same position and attitude calculation method, and the variable superscript 1 in step 2 in 1) to 8) is replaced by 2, that is, the rocker arm The position and attitude calculation method of the coupled inertial navigation device 3; Step 4. Calculate the relative distance between the shearer body and the rocker arm The rocker arm wireless sensor mobile node 4 measures the distance relative to the fuselage wireless sensor anchor node 2: The rocker arm wireless sensor mobile node 4 transmits wireless signals, the fuselage wireless sensor anchor node 2 receives the wireless signal of the mobile node, and uses the RSSI algorithm for ranging, and the RSSI-based ranging method converts the received signal strength into wireless transmission. sense the distance between the mobile node 4 and the wireless sensing anchor node 2, using the well-known log-constant wireless signal propagation model, S=A-10mlg(d) (8) Among them, S is the signal strength, d is the distance, A is the received wireless signal strength when the receiving end is 1m away from the transmitting end, m is the path loss, A and m are known parameters, and the distance d is calculated according to S; According to formula (8), according to the signal strength S received by the wireless sensor anchor node 2, the distance d of the rocker arm relative to the fuselage can be obtained; When the wireless sensor mobile node 4 transmits a wireless signal, the rocker arm SINS device 3 records the position information calculated by the inertial navigation at that moment, and sends it to the fuselage SINS device 1; the fuselage wireless sensor anchor node 2. When receiving the wireless signal, receive the position information at the time of ranging sent by the rocker arm strapdown inertial navigation device 3, and simultaneously record the body position and attitude information and the rocker arm position and attitude information calculated by the fuselage strapdown inertial navigation device 1; Step 5: Use the fuselage position and attitude information, the rocker arm position and attitude information, and the relative distance between the fuselage and the rocker arm recorded at the time of transmitting the wireless ranging signal in step 4, and perform collaborative navigation filtering based on Kalman filter. Estimate the position, velocity and attitude errors of the rocker arm SINS 3; 1) The state variable of the collaborative navigation filter is

Wherein ΔP _I =[ΔP _N ΔP _U ΔP _E ] ^T represents the position error of the rocker arm SINS 3; ΔV=[ΔV _N ΔV _U ΔV _E ] ^T represents the speed error of the rocker arm SINS 3; φ= [φ _N φ _U φ _E ] ^T represents the attitude error of the rocker arm strapdown inertial navigation device 3; 2) The cooperative navigation filtering state equation is composed of the velocity error equation, position error equation and attitude error equation of the rocker arm SINS device 3: Velocity Error Equation:

in,

Position Error Equation

Attitude Error Equation

The equations (9), (10) and (11) are discretized and rewritten into the form of cooperative navigation filtering state equation, where t _k represents the filtering time point; X(t _k )=F(t _k-1 )X(t _k-1 ) (12) 3) The collaborative navigation filtering filtering measurement equation is as follows:

4) Correct the 1-lead synchronization error of the fuselage strapdown inertial device The update rate of the positioning data of the fuselage strapdown inertial navigation device 1 is higher than that of the wireless ranging, and the linear interpolation method can be used. When the rocker arm wireless sensing mobile node 4 transmits wireless signals, the fuselage strapdown inertial navigation 1. Record the time interval Δt, Δt<ΔT, and use the interpolation method to calculate the position P ¹ of the fuselage strapdown inertial navigation device 1 used for cooperative navigation:

5) Calculate the amount of measurement Z At the moment when the wireless ranging signal is transmitted, the position of the fuselage wireless sensor anchor node 2 (that is, the position of the fuselage strapdown inertial navigation device) is

When the rocker arm wireless sensor moves 4 nodes, that is, the rocker arm strapdown inertial navigation device 3 position is:

According to the geometric relationship there are:

Among them, Y represents the relative distance calculated by using the position of the rocker arm SINS device 3 and the position of the fuselage SINS device 1 . Then the quantity measurement Z of the collaborative navigation filtering is calculated as follows: Z=Y-d

6) The fuselage strapdown inertial navigation device 1 uses the well-known Kalman filter algorithm to obtain the estimated value of the cooperative navigation filter state quantity

Step 6. Use collaborative navigation to filter state estimates

Correct the position, speed and attitude errors of the rocker arm strapdown inertial navigation device 3, and obtain the position, speed and attitude of the rocker arm after error correction: The fuselage strapdown inertial navigation device 1 will be

Send it to the rocker arm strapdown inertial navigation device 3, and perform closed-loop point correction for its error: P ² (t _k )=P ² (t _k )-ΔP(t _k ) (17) V ² (t _k )=V ² (t _k )-ΔV(t _k ) (18)

The position, velocity and attitude matrix of device 3 after error correction is obtained, and the error correction

According to 6) of step 2, replace the variable superscript 1 with 2, and calculate the attitude angle of the rocker arm after error correction: pitch angle θ ² , heading angle ψ ² and roll angle γ ² .

Images