CN111323011A - Coal mining machine body and rocker arm cooperative positioning device and positioning method - Google Patents
Coal mining machine body and rocker arm cooperative positioning device and positioning method Download PDFInfo
- Publication number
- CN111323011A CN111323011A CN202010327977.XA CN202010327977A CN111323011A CN 111323011 A CN111323011 A CN 111323011A CN 202010327977 A CN202010327977 A CN 202010327977A CN 111323011 A CN111323011 A CN 111323011A
- Authority
- CN
- China
- Prior art keywords
- rocker arm
- inertial navigation
- strapdown inertial
- navigation device
- coordinate system
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
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
Technical Field
The invention belongs to the technical field of positioning of coal mining machines, and particularly relates to a device and a method for cooperatively positioning a coal mining machine body and a rocker arm.
Background
The coal cutter, the hydraulic support and the scraper conveyor are three most important devices of the underground fully mechanized coal mining face, and are matched with each other to finish coal cutting, coal conveying and supporting. The coal mining machine is a leading device, is a main device for cutting and loading coal on a fully mechanized coal mining face, and is a high-integration fully mechanized coal mining device. When the coal cutter works, coal is cut in a reciprocating mode along the track of the scraper conveyor, and the hydraulic support supports the top plate and pushes the working face. In order to realize the automation and remote automatic control of the fully mechanized coal mining face, the coal mining machine needs to be accurately and dynamically positioned, and the machine body and the rocker arm of the coal mining machine need to be positioned simultaneously according to the requirement of automatic coal mining, so the positioning technology of the machine body and the rocker arm of the coal mining machine is the key technology for the automation of coal mine production equipment.
The working condition of the fully mechanized coal mining face of the coal mine is complex, and the space is closed, so that the positioning of the machine body and the rocker arm of the coal mining machine is a typical indoor positioning problem in a complex closed environment. In the coal mining operation of the coal mining machine, a machine body of the coal mining machine moves along a track; and the rocker arm performs coal cutting work and has relative motion relative to the coal cutter body. In order to realize automatic coal mining, the positions and attitudes of the shearer body and the ranging arm must be measured simultaneously.
The positioning method mainly comprises a strapdown inertial navigation positioning method, an infrared positioning method, an ultrasonic positioning method, a gear counting positioning method, a wireless sensor network positioning method and the like.
The infrared positioning method is characterized in that an infrared transmitting device arranged on a coal mining machine transmits signals, a receiving device arranged on a hydraulic support receives the signals, and the position of the coal mining machine is positioned by utilizing infrared distance measurement.
The ultrasonic positioning method is characterized in that an ultrasonic transmitting device is installed in a roadway of a working face, when a coal mining machine passes through, a machine body transmits ultrasonic waves, the ultrasonic receiving devices receive signals according to all positions, the positions of the coal mining machine are located by utilizing ultrasonic ranging, and the ultrasonic waves have the advantages of penetrating through dust, but because the working face is long, signal loss is serious, the locating precision is not high, and the use is limited.
The gear counting and positioning method is to count the number of turns of the walking gear of the coal mining machine and calculate the displacement of the coal mining machine along the track direction of the conveyor according to the number of turns and the circumference of the gear; however, the method can only be used for positioning the one-dimensional position of the coal mining machine along the track direction, is influenced by the gear counting error, and cannot meet the three-dimensional positioning requirement.
The wireless sensor network positioning method is characterized in that a plurality of wireless sensors (called anchor nodes) with known positions are arranged on a hydraulic support, a node to be positioned (called mobile node) is arranged on a coal mining machine, the mobile node transmits a wireless signal, the anchor nodes receive the wireless signal to monitor the position relation between the coal mining machine and the hydraulic support, and the position of the coal mining machine is calculated; however, due to the fact that the working face environment is complex, wireless positioning data are unstable, the anchor nodes can change positions after moving along with the hydraulic support, anchor node position information needs to be updated, meanwhile, the coal mining machine cannot perform attitude determination, and the requirements for real-time positioning and attitude determination cannot be met.
The strapdown inertial navigation positioning method is a full-autonomous navigation positioning method, without the help of external information, the angular velocity and the linear acceleration of the coal mining machine are measured in real time by utilizing a three-axis gyroscope and a three-axis accelerometer of a strapdown inertial navigation device, the motion attitude of the coal mining machine is firstly solved through an attitude updating algorithm by combining initial binding information, then the acceleration is projected to a navigation coordinate system according to the attitude information, and information such as the velocity and the position of the coal mining machine is obtained through integral and secondary integral; although the high-precision inertial navigation meets the precision requirement, the high-precision inertial navigation is generally only installed on a machine body of a coal mining machine due to the reasons of volume, weight and the like and cannot be used for positioning and attitude determination of a rocker arm of the coal mining machine, or the rocker arm of the coal mining machine can only be installed with small-sized low-precision inertial navigation, and the positioning and attitude determination precision cannot meet the requirement.
Disclosure of Invention
In order to overcome the problems in the prior art, the invention provides a device and a method for cooperatively positioning a coal mining machine body and a rocker arm, which can simultaneously realize the positioning and attitude determination of the coal mining machine body and the rocker arm by utilizing a strapdown inertial navigation device and a wireless distance measuring sensor on the coal mining machine body and the rocker arm, and have the advantages of high positioning and attitude determination precision, high fully mechanized mining work efficiency and remote automatic control.
In order to achieve the purpose, the invention adopts the following technical scheme:
a coal mining machine body and rocker arm cooperative positioning device comprises a machine body strapdown inertial navigation device 1 and a wireless sensing anchor node 2 which are arranged on a coal mining machine body 5; the rocker arm strapdown inertial navigation device 3 and the wireless sensing mobile node 4 are arranged on a rocker arm 6 on the coal cutting side of the coal cutter; wherein the wireless sensing mobile node 4 transmits wireless signals, and the wireless sensing anchor node 2 receives the wireless signals.
The fuselage strapdown inertial navigation device 1 comprises three gyros and three accelerometers.
The positioning method based on the coal mining machine body and rocker arm cooperative positioning device specifically comprises the following steps:
step one, defining a coordinate system
1) Defining a coordinate system b1 of the body of the coal mining machine: o isb1Xb1Yb1Zb1: origin O of coordinate systemb1Fixedly connected to the center of the strapdown inertial navigation device 1 of the machine body, Xb1The axis being directed forwardly from the shearer toward the coal wall, Yb1Axis perpendicular to Xb1In the axial direction, Zb1Axis and Xb1Axis, Yb1The axes form a right-hand coordinate system to form a front upper right coordinate system, and the subscript 1 represents the coordinate system as a machine body coordinate system;
2) defining a coordinate system b2 of the rocker arm of the coal mining machine: o isb2Xb2Yb2Zb2(ii) a Origin O of coordinate systemb2Is fixedly connected at the center of a rocker arm strapdown inertial navigation device 3, Xb2The shaft is positively directed from the rocker arm to the coal wall, Yb2Axis perpendicular to Xb2In the axial direction, Zb2Axis and Xb2Axis, Yb2The axes form a right-hand coordinate system to form a front upper right coordinate system, and the subscript 2 represents the coordinate system as a rocker arm coordinate system;
3) defining a navigation coordinate system n: o isnXnYnZn: North-Tiandong geographic coordinate System, XnThe axis pointing to the geographical north, YnThe axis pointing in the sky direction, ZnThe axis points in the geographic east direction;
4) navigation coordinate system n is rotated three times and then coal mining is carried outThe coordinate systems b1 of the airframe are overlapped, and the angle of the three rotations is the heading angle psi of the airframe1Angle of pitch theta1And roll angle gamma1(ii) a Similarly, the navigation coordinate system n is coincided with a coal mining machine rocker arm coordinate system b2 after three times of rotation, and the angle of the three times of rotation is the rocker arm heading angle psi2Angle of pitch theta2And roll angle gamma2;
Step two, calculating the information of the coal mining machine body
The strapdown inertial navigation device 1 of the coal mining machine body utilizes the gyro and the accelerometer to measure information and calculates the attitude pitch angle theta of the coal mining machine body1Heading angle psi1And roll angle gamma1Calculating the speed of the bodyPosition ofAnd longitude λ1Latitude and longitudeHeight h1;
1) The machine body strapdown inertial navigation device 1 receives initial binding information: initial positionAnd initial attitudeWherein the superscript 1 represents the strapdown inertial navigation of the fuselage, and N, U and E respectively represent the north, the sky and the east positions under the navigation coordinate system; meanwhile, the coal mining machine is initially in a stop state, and the initial speed V is1=[0 0 0]T;
2) Three gyros of the body strapdown inertial navigation device 1 measure angular velocity vectors of three axes of the body of the coal mining machineThree accelerometers for measuring acceleration vectors of three axial directions of coal mining machine body
3) The body strapdown inertial navigation device 1 firstly updates the angular velocity vector of the b1 coordinate system of the coal mining machine body relative to the navigation coordinate system n
Wherein the content of the first and second substances,the latitude of the strapdown inertial navigation device 1 of the machine body is shown,is the attitude matrix, omega, of the fuselage strapdown inertial navigation device 1ieIs the angular velocity of the earth rotation, R is the radius of the earth,andrepresenting the east speed and the north speed of the body strapdown inertial navigation device 1;
4) updating quaternion q1=[q0q1q2q3]T
5) updating the attitude matrix of the fuselage strapdown inertial navigation device 1 relative to the navigation coordinate system
pitch angle theta1=sin-1(C12)
7) By usingThe acceleration vector output by the accelerometerProjection onto a navigation coordinate system
8) Updating the speed of the fuselage strapdown inertial navigation unit 1Position ofAnd longitude λ1Latitude and longitudeHeight h1,
Step three, calculating rocker arm information of coal mining machine
The coal mining machine rocker arm strapdown inertial navigation device 3 utilizes the gyro and accelerometer to measure information to calculate the attitude pitch angle theta of the rocker arm of the coal mining machine2Heading angle psi2And roll angle gamma2Calculating the velocity of the rocker armPosition ofAnd longitude λ2Latitude and longitudeHeight h2;
The rocker arm strapdown inertial navigation device 3 receives initial binding information: initial positionAnd initial attitudeThe upper mark 2 represents rocker arm strapdown inertial navigation, and N, U and E respectively represent the north, the sky and the east positions under a navigation coordinate system; at the same time, the coal mining machine is at the initial positionAt a stopped state, an initial speed V2=[0 0 0]T;
The position and attitude calculation method of the rocker arm strapdown inertial navigation device 3 and the fuselage strapdown inertial navigation device 1 are the same, and the variable superscript 1 in the steps 1) to 8) in the step two is changed into 2, namely the position and attitude calculation method of the rocker arm strapdown inertial navigation device 3 is obtained;
step four, calculating the relative distance between the coal mining machine body and the rocker arm
Ranging of the rocker arm wireless sensing mobile node 4 relative to the fuselage wireless sensing anchor node 2:
the rocker arm wireless sensing mobile node 4 transmits a wireless signal, the body wireless sensing anchor node 2 receives the wireless signal of the mobile node, the RSSI algorithm is utilized to carry out distance measurement, the RSSI-based distance measurement method converts the received signal strength into the distance between the wireless sensing mobile node 4 and the wireless sensing anchor node 2, a known logarithm-constant wireless signal propagation model is used,
S=A-10m lg(d) (8)
wherein S is signal intensity, d is distance, A is wireless signal intensity received when the distance between the receiving end and the transmitting end is 1m, m is path loss, A and m are known parameters, and the distance d is calculated according to S;
according to a formula (8), the distance d of the rocker arm relative to the machine body can be obtained according to the signal intensity S received by the wireless sensing anchor node 2;
when the wireless sensing mobile node 4 transmits a wireless signal, the rocker arm strapdown inertial navigation device 3 records position information resolved by inertial navigation at the moment and sends the position information to the fuselage strapdown inertial navigation device 1; when the wireless sensing anchor node 2 of the machine body receives a wireless signal, the position information at the ranging time sent by the rocker arm strapdown inertial navigation device 3 is received, and the position and attitude information of the machine body and the position and attitude information of the rocker arm calculated by the machine body strapdown inertial navigation device 1 are recorded.
Fifthly, utilizing the position and attitude information of the machine body, the position and attitude information of the rocker arm and the relative distance between the machine body and the rocker arm recorded at the time of transmitting the wireless ranging signal in the fourth step to carry out collaborative navigation filtering calculation based on a Kalman filter, and estimating the position, the speed and the attitude error of the rocker arm strapdown inertial navigation device 3;
1) the state variable of the collaborative navigation filter isWhereinThe position error of the rocker arm strapdown inertial navigation device 3 is shown;representing the speed error of the rocker arm strapdown inertial navigation device 3; phi is ═ phiNφUφE]TRepresenting the attitude error of the rocker arm strapdown inertial navigation device 3;
2) the collaborative navigation filtering state equation consists of a speed error equation, a position error equation and an attitude error equation of the rocker arm strapdown inertial navigation device 3:
the velocity error equation:
wherein the content of the first and second substances,
equation of position error
Equation of attitude error
Discretizing and rewriting equations (9), (10), (11) into a collaborative navigation filtering state equation form, wherein tkRepresenting a filtering time point;
X(tk)=F(tk-1)X(tk-1) (12)
3) the collaborative navigation filtering measurement equation is as follows:
4) correcting the synchronous error of the strapdown inertial device 1
The updating rate of the positioning data of the body strapdown inertial navigation device 1 is higher than that of the positioning data of the wireless distance measurement, a linear interpolation method can be adopted, when the rocker arm wireless sensing mobile node 4 transmits a wireless signal, the time interval delta T is recorded by the body strapdown inertial navigation device 1, the delta T is less than the delta T, and the position P of the body strapdown inertial navigation device 1 for collaborative navigation is calculated by adopting the interpolation method1:
5) Calculated quantity measurement Z
At the moment of transmitting the wireless ranging signal, the position of the wireless sensing anchor node 2 of the machine body (namely the position of the strapdown inertial navigation device of the machine body) isThe position of the rocker arm wireless sensing mobile node 4, namely the position of the rocker arm strapdown inertial navigation device 3 isAccording to the geometrical relationship:
wherein, Y represents the relative distance calculated by using the position of the rocker arm strapdown inertial navigation device 3 and the position of the fuselage strapdown inertial navigation device 1. The amount of co-navigation filtering, Z, is then calculated as follows:
Z=Y-d
6) the fuselage strapdown inertial navigation device 1 can obtain the estimated value of the collaborative navigation filtering state quantity by utilizing the well-known Kalman filtering algorithm
Sixthly, utilizing the collaborative navigation filtering state estimation valueCorrecting the position, speed and attitude errors of the rocker arm strapdown inertial navigation device 3 to obtain the position, speed and attitude of the rocker arm after error correction:
the body strapdown inertial navigation device 1 willSending the error to a rocker arm strapdown inertial navigation device 3, and carrying out closed-loop point correction on the error:
P2(tk)=P2(tk)-ΔP(tk) (17)
V2(tk)=V2(tk)-ΔV(tk) (18)
obtaining the position, speed and attitude matrix of the error-corrected rocker arm strapdown inertial navigation device 3, and correcting the errorAccording to step two, 6), replacing the variable superscript 1 with 2, and calculating the posture angle of the rocker arm after error correction: pitch angle theta2Heading angle psi2And roll angle gamma2。
The invention has the beneficial effects that: the method for cooperatively positioning the coal mining machine body 5 and the rocker arm 6 is provided, the high-precision machine body strapdown inertial navigation device 1 calculates the position and the posture of the machine body, the rocker arm strapdown inertial navigation device 3 calculates the position and the posture of the rocker arm, the position and the posture errors of the rocker arm are estimated and corrected by utilizing a cooperative navigation filtering method through wireless distance measurement between the machine body wireless sensing anchor node 2 and the rocker arm wireless sensing mobile node 4, and the high-precision positioning and posture fixing of the coal mining machine body 5 and the rocker arm 6 are realized, so that the requirements of automation and remote automatic control of a fully mechanized mining working surface are met.
Drawings
FIG. 1 is a schematic view of the co-location of a shearer body and a ranging arm.
Fig. 2 is a schematic diagram of a coordinate system of a body (rocker arm), a navigation coordinate system and a posture.
Fig. 3 is a flow chart of the co-location work of the body and the rocker arm of the coal mining machine.
Fig. 4 is a timing diagram of positioning information of the coal mining machine inertial/wireless sensor network combination.
In the figure: 1. a fuselage strapdown inertial navigation device; 2. a fuselage wireless sensing anchor node; 3. a rocker arm strapdown inertial navigation device; 4. a rocker arm wireless sensing mobile node; 5. a body; 6. a rocker arm.
Detailed Description
The present invention will be described in further detail with reference to the accompanying drawings.
Referring to fig. 1, a coal mining machine body and rocker arm co-location device comprises a machine body strapdown inertial navigation device 1 and a wireless sensing anchor node 2 which are installed on the coal mining machine body; the rocker arm strapdown inertial navigation device 3 and the wireless sensing mobile node 4 are arranged on a rocker arm 6 on the coal cutting side of the coal cutter; wherein the wireless sensing mobile node 4 transmits wireless signals, and the wireless sensing anchor node 2 receives the wireless signals.
The fuselage strapdown inertial navigation device 1 comprises three gyros and three accelerometers.
The positioning method based on the coal mining machine body and rocker arm cooperative positioning device has the following specific implementation mode:
step one, defining a coordinate system
1) Defining a coordinate system b1 of the body of the coal mining machine: o isb1Xb1Yb1Zb1: origin O of coordinate systemb1Strapdown inertial measurement unit fixedly connected to machine bodyGuide 1 center, Xb1The axis being directed forwardly from the shearer toward the coal wall, Yb1Axis perpendicular to Xb1In the axial direction, Zb1Axis and Xb1Axis, Yb1The axes form a right-hand coordinate system to form a front upper right coordinate system, and the subscript 1 represents the coordinate system as a machine body coordinate system;
2) defining a coordinate system b2 of the rocker arm of the coal mining machine: o isb2Xb2Yb2Zb2(ii) a Origin O of coordinate systemb2Is fixedly connected at the center of the rocker arm strapdown inertial navigation device and is Xb2The shaft is positively directed from the rocker arm to the coal wall, Yb2Axis perpendicular to Xb2In the axial direction, Zb2Axis and Xb2Axis, Yb2The axes form a right-hand coordinate system to form a front upper right coordinate system, and the subscript 2 represents the coordinate system as a rocker arm coordinate system;
3) defining a navigation coordinate system n: o isnXnYnZn: North-Tiandong geographic coordinate System, XnThe axis pointing to the geographical north, YnThe axis pointing in the sky direction, ZnThe axis points in the geographic east direction;
4) the navigation coordinate system n is superposed with a coordinate system b1 of the body of the coal mining machine after three times of rotation, and the angle of the three times of rotation is the heading angle psi of the body1Angle of pitch theta1And roll angle gamma1As shown in fig. 2; similarly, the navigation coordinate system n is coincided with a coal mining machine rocker arm coordinate system b2 after three times of rotation, and the angle of the three times of rotation is the rocker arm heading angle psi2Angle of pitch theta2And roll angle gamma2;
Step two, calculating the information of the coal mining machine body
The strapdown inertial navigation device 1 of the coal mining machine body utilizes the gyro and the accelerometer to measure information and calculates the attitude pitch angle theta of the coal mining machine body1Heading angle psi1And roll angle gamma1Calculating the speed of the bodyPosition ofAnd longitude λ1Latitude and longitudeHeight h1;
1) The machine body strapdown inertial navigation device 1 receives initial binding information: initial positionAnd initial attitudeWherein the superscript 1 represents the strapdown inertial navigation of the fuselage, and N, U and E respectively represent the north, the sky and the east positions under the navigation coordinate system; meanwhile, the coal mining machine is initially in a stop state, and the initial speed V is1=[0 0 0]T;
2) Three gyros of the body strapdown inertial navigation device 1 measure angular velocity vectors of three axes of the body of the coal mining machineThree accelerometers for measuring acceleration vectors of three axial directions of coal mining machine body
3) The body strapdown inertial navigation device 1 firstly updates the angular velocity vector of the b1 coordinate system of the coal mining machine body relative to the navigation coordinate system n
Wherein the content of the first and second substances,the latitude of the strapdown inertial navigation device 1 of the machine body is shown,is the attitude matrix, omega, of the fuselage strapdown inertial navigation device 1ieIs the angular velocity of rotation of the earthR is the radius of the earth,andrepresenting the east speed and the north speed of the body strapdown inertial navigation device 1;
4) updating quaternion q1=[q0q1q2q3]T
5) updating the attitude matrix of the fuselage strapdown inertial navigation device 1 relative to the navigation coordinate system
pitch angle theta1=sin-1(C12)
7) By usingThe acceleration vector output by the accelerometerProjection onto a navigation coordinate system
8) Updating the speed of the fuselage strapdown inertial navigation unit 1Position ofAnd longitude λ1Latitude and longitudeHeight h1,
Step three, calculating rocker arm information of coal mining machine
The coal mining machine rocker arm strapdown inertial navigation device 3 utilizes the gyro and accelerometer to measure information to resolve the coal mining machine rocker armAttitude pitch angle θ2Heading angle psi2And roll angle gamma2Calculating the velocity of the rocker armPosition ofAnd longitude λ2Latitude and longitudeHeight h2;
The rocker arm strapdown inertial navigation device 3 receives initial binding information: initial positionAnd initial attitudeThe upper mark 2 represents rocker arm strapdown inertial navigation, and N, U and E respectively represent the north, the sky and the east positions under a navigation coordinate system; meanwhile, the coal mining machine is initially in a stop state, and the initial speed V is2=[0 0 0]T;
The position and attitude calculation method of the rocker arm strapdown inertial navigation device 3 and the fuselage strapdown inertial navigation device 1 are the same, and the variable superscript 1 in the steps 1) to 8) in the step two is changed into 2, namely the position and attitude calculation method of the rocker arm strapdown inertial navigation device 3 is obtained;
step four, calculating the relative distance between the coal mining machine body and the rocker arm
Ranging of the rocker arm wireless sensing mobile node 4 relative to the fuselage wireless sensing anchor node 2:
the rocker arm wireless sensing mobile node 4 transmits a wireless signal, the body wireless sensing anchor node 2 receives the wireless signal of the mobile node, the RSSI algorithm is utilized to carry out distance measurement, the RSSI-based distance measurement method converts the received signal strength into the distance between the wireless sensing mobile node 4 and the wireless sensing anchor node 2, a known logarithm-constant wireless signal propagation model is used,
S=A-10m lg(d) (8)
wherein S is signal intensity, d is distance, A is wireless signal intensity received when the distance between the receiving end and the transmitting end is 1m, m is path loss, A and m are known parameters, and the distance d is calculated according to S;
according to a formula (8), the distance d of the rocker arm relative to the machine body can be obtained according to the signal intensity S received by the wireless sensing anchor node 2;
when the wireless sensing mobile node 4 transmits a wireless signal, the rocker arm strapdown inertial navigation device 3 records position information resolved by inertial navigation at the moment and sends the position information to the fuselage strapdown inertial navigation device 1; when the machine body wireless sensing anchor node 2 receives a wireless signal, the ranging time position information sent by the rocker arm strapdown inertial navigation device 3 is received, and the machine body position attitude information and the rocker arm position attitude information resolved by the machine body strapdown inertial navigation device 1 are recorded;
fifthly, utilizing the position and attitude information of the machine body, the position and attitude information of the rocker arm and the relative distance between the machine body and the rocker arm recorded at the time of transmitting the wireless ranging signal in the fourth step to carry out collaborative navigation filtering calculation based on a Kalman filter, and estimating the position, the speed and the attitude error of the rocker arm strapdown inertial navigation device 3;
1) the state variable of the collaborative navigation filter isWherein Δ PI=[ΔPNΔPUΔPE]TThe position error of the rocker arm strapdown inertial navigation device 3 is shown; Δ V ═ Δ VNΔVUΔVE]TRepresenting the speed error of the rocker arm strapdown inertial navigation device 3; phi is ═ phiNφUφE]TRepresenting the attitude error of the rocker arm strapdown inertial navigation device 3;
2) the collaborative navigation filtering state equation consists of a speed error equation, a position error equation and an attitude error equation of the rocker arm strapdown inertial navigation device 3:
the velocity error equation:
wherein the content of the first and second substances,
equation of position error
Equation of attitude error
Discretizing and rewriting equations (9), (10), (11) into a collaborative navigation filtering state equation form, wherein tkRepresenting a filtering time point;
X(tk)=F(tk-1)X(tk-1) (12)
3) the collaborative navigation filtering measurement equation is as follows:
4) correcting the synchronous error of the strapdown inertial device 1
The updating rate of the positioning data of the body strapdown inertial navigation device 1 is higher than that of the positioning data of wireless distance measurement, an interpolation method shown in figure 4 is adopted, when the rocker arm wireless sensing mobile node 4 transmits a wireless signal, the body strapdown inertial navigation device 1 records a time interval delta T, the delta T is less than the delta T, and the position P of the body strapdown inertial navigation device 1 for collaborative navigation is calculated by adopting the interpolation method1:
5) Calculated quantity measurement Z
At the moment of transmitting the wireless ranging signal, the body transmits wirelesslyThe position of the anchor sensing node 2 (namely the position of the strapdown inertial navigation device of the machine body) isThe position of the rocker arm wireless sensing mobile node 4, namely the position of the rocker arm strapdown inertial navigation device 3 isAccording to the geometrical relationship:
wherein, Y represents the relative distance calculated by using the position of the rocker arm strapdown inertial navigation device 3 and the position of the fuselage strapdown inertial navigation device 1. The amount of co-navigation filtering, Z, is then calculated as follows:
Z=Y-d
6) the fuselage strapdown inertial navigation device 1 can obtain the estimated value of the collaborative navigation filtering state quantity by utilizing the well-known Kalman filtering algorithm
Sixthly, utilizing the collaborative navigation filtering state estimation valueCorrecting the position, speed and attitude errors of the rocker arm strapdown inertial navigation device 3 to obtain the position, speed and attitude of the rocker arm after error correction:
the body strapdown inertial navigation device 1 willSending the error to a rocker arm strapdown inertial navigation device 3, and carrying out closed-loop point correction on the error:
P2(tk)=P2(tk)-ΔP(tk) (17)
V2(tk)=V2(tk)-ΔV(tk) (18)
obtaining the position, speed and attitude matrix of the error-corrected rocker arm strapdown inertial navigation device 3, and correcting the errorAccording to step two, 6), replacing the variable superscript 1 with 2, and calculating the posture angle of the rocker arm after error correction: pitch angle theta2Heading angle psi2And roll angle gamma2。
Referring to fig. 3, the working principle of the present invention is:
in the coal mining operation of the coal mining machine, the coal mining machine body 5 moves along the track, and the rocker arm 6 performs coal wall cutting movement; the rocker arm strapdown inertial navigation device 3 measures the angular velocity and acceleration information of the rocker arm of the coal mining machine, and carries out positioning and attitude determination by utilizing a strapdown navigation algorithm; the rocker arm wireless sensing mobile node 4 transmits a wireless signal, and meanwhile, the rocker arm strapdown inertial navigation device 3 transmits the inertial information at the moment to the machine body strapdown inertial navigation device 1; the body strapdown inertial navigation device 1 measures the angular velocity and acceleration information of the body of the coal mining machine in real time, and carries out positioning and attitude determination by utilizing a strapdown navigation algorithm; the machine body wireless sensing anchor node 2 receives wireless signals of the rocker arm wireless sensing mobile node 4, and distance measurement calculation is carried out by utilizing an RSSI algorithm; the machine body strapdown inertial navigation device 1 resolves the machine body inertial information at the arrival time of the wireless signal, receives the inertial information of the rocker arm strapdown inertial navigation device 3, performs collaborative navigation filtering with the ranging and positioning result of the machine body wireless sensing anchor node 2, and estimates the position and attitude error of the rocker arm; and according to the error estimation result, sending the corrected position and posture information of the rocker arm to a rocker arm strapdown inertial navigation device, and correcting the positioning and posture-fixing errors of the rocker arm, thereby obtaining the accurate position and posture information of the body and the rocker arm of the coal mining machine at the same time.
It should be noted that although the present invention uses a strapdown inertial navigation device as a specific implementation example, the present invention only needs to slightly modify the navigation solution from step one to step six, and is also applicable to platform inertial navigation and other forms of inertial navigation devices.
It should be understood that the above embodiments are only for illustrating the technical solutions of the present invention and not for limiting the same, and although the present invention is described in detail with reference to the above embodiments, those of ordinary skill in the art should understand that: modifications and equivalents may be made to the embodiments of the invention without departing from the spirit and scope of the invention, which is to be covered by the claims.
Claims (3)
1. The utility model provides a coal-winning machine fuselage and rocking arm positioner in coordination which characterized in that: the system comprises a machine body strapdown inertial navigation device (1) and a wireless sensing anchor node (2) which are arranged on a machine body (5) of the coal mining machine; a rocker arm strapdown inertial navigation device (3) and a wireless sensing mobile node (4) are arranged on a rocker arm (6) on the coal cutting side of the coal cutter; the wireless sensing mobile node (4) transmits wireless signals, and the wireless sensing anchor node (2) receives the wireless signals.
2. The co-locating device for the body and the rocker arm of the coal mining machine according to claim 1, characterized in that: the strapdown inertial navigation device (1) comprises three gyros and three accelerometers.
3. The positioning method of the coal mining machine body and rocker arm co-positioning device based on claim 1 or 2 specifically comprises the following steps:
step one, defining a coordinate system
1) Defining a coordinate system b1 of the body of the coal mining machine: o isb1Xb1Yb1Zb1: origin O of coordinate systemb1Fixedly connected to the center of the strapdown inertial navigation device 1 of the machine body, Xb1The axis being directed forwardly from the shearer toward the coal wall, Yb1Axis perpendicular to Xb1In the axial direction, Zb1Axis and Xb1Axis, Yb1The axes form a right-hand coordinate system to form a front upper right coordinate system, and the subscript 1 represents the coordinate system as a machine body coordinate system;
2) defining a coordinate system b2 of the rocker arm of the coal mining machine: o isb2Xb2Yb2Zb2(ii) a Origin O of coordinate systemb2Is fixedly connected at the center of the rocker arm strapdown inertial navigation device and is Xb2The shaft is positively directed from the rocker arm to the coal wall, Yb2Axis perpendicular to Xb2In the axial direction, Zb2Axis and Xb2Axis, Yb2The axes form a right-hand coordinate system to form a front upper right coordinate system, and the subscript 2 represents the coordinate system as a rocker arm coordinate system;
3) defining a navigation coordinate system n: o isnXnYnZn: North-Tiandong geographic coordinate System, XnThe axis pointing to the geographical north, YnThe axis pointing in the sky direction, ZnThe axis points in the geographic east direction;
4) the navigation coordinate system n is superposed with a coordinate system b1 of the body of the coal mining machine after three times of rotation, and the angle of the three times of rotation is the heading angle psi of the body1Angle of pitch theta1And roll angle gamma1(ii) a Similarly, the navigation coordinate system n is coincided with a coal mining machine rocker arm coordinate system b2 after three times of rotation, and the angle of the three times of rotation is the rocker arm heading angle psi2Angle of pitch theta2And roll angle gamma2;
Step two, calculating the information of the coal mining machine body
The strapdown inertial navigation device 1 of the coal mining machine body utilizes the gyro and the accelerometer to measure information and calculates the attitude pitch angle theta of the coal mining machine body1Heading angle psi1And roll angle gamma1Calculating the speed of the bodyPosition ofAnd longitude λ1Latitude and longitudeHeight h1;
1) The machine body strapdown inertial navigation device 1 receives initial binding information: initial positionAnd initial attitudeWherein the superscript 1 represents the strapdown inertial navigation of the fuselage, and N, U and E respectively represent the north, the sky and the east positions under the navigation coordinate system; meanwhile, the coal mining machine is initially in a stop state, and the initial speed V is1=[0 0 0]T;
2) The body strapdown inertial navigation device 1 comprises three gyroscopes and three accelerometers, wherein the three gyroscopes are used for measuring angular velocity vectors of three axes of the body of the coal mining machineThree accelerometers for measuring acceleration vectors of three axial directions of coal mining machine body
3) The body strapdown inertial navigation device 1 firstly updates the angular velocity vector of the b1 coordinate system of the coal mining machine body relative to the navigation coordinate system n
Wherein the content of the first and second substances,the latitude of the strapdown inertial navigation device 1 of the machine body is shown,is the attitude matrix, omega, of the fuselage strapdown inertial navigation device 1ieIs the angular velocity of the earth rotation, R is the radius of the earth,andrepresenting the east speed and the north speed of the body strapdown inertial navigation device 1;
4) updating quaternion q1=[q0q1q2q3]T
5) updating the attitude matrix of the fuselage strapdown inertial navigation device 1 relative to the navigation coordinate system
pitch angle theta1=sin-1(C12)
7) By usingThe acceleration vector output by the accelerometerProjection onto a navigation coordinate system
8) Updating the speed of the fuselage strapdown inertial navigation unit 1Position ofAnd longitude λ1Latitude and longitudeHeight h1,
Step three, calculating rocker arm information of coal mining machine
The coal mining machine rocker arm strapdown inertial navigation device 3 utilizes the gyro and accelerometer to measure information to calculate the attitude pitch angle theta of the rocker arm of the coal mining machine2Heading angle psi2And roll angle gamma2Calculating the velocity of the rocker armPosition ofAnd longitude λ2Latitude and longitudeHeight h2;
The rocker arm strapdown inertial navigation device 3 receives initial binding information: initial positionAnd initial attitudeThe upper mark 2 represents rocker arm strapdown inertial navigation, and N, U and E respectively represent the north, the sky and the east positions under a navigation coordinate system; meanwhile, the coal mining machine is initially in a stop state, and the initial speed V is2=[0 0 0]T;
The position and attitude calculation method of the rocker arm strapdown inertial navigation device 3 and the fuselage strapdown inertial navigation device 1 are the same, and the variable superscript 1 in the steps 1) to 8) in the step two is changed into 2, namely the position and attitude calculation method of the rocker arm strapdown inertial navigation device 3 is obtained;
step four, calculating the relative distance between the coal mining machine body and the rocker arm
Ranging of the rocker arm wireless sensing mobile node 4 relative to the fuselage wireless sensing anchor node 2:
the rocker arm wireless sensing mobile node 4 transmits a wireless signal, the body wireless sensing anchor node 2 receives the wireless signal of the mobile node, the RSSI algorithm is utilized to carry out distance measurement, the RSSI-based distance measurement method converts the received signal strength into the distance between the wireless sensing mobile node 4 and the wireless sensing anchor node 2, a known logarithm-constant wireless signal propagation model is used,
S=A-10mlg(d) (8)
wherein S is signal intensity, d is distance, A is wireless signal intensity received when the distance between the receiving end and the transmitting end is 1m, m is path loss, A and m are known parameters, and the distance d is calculated according to S;
according to a formula (8), the distance d of the rocker arm relative to the machine body can be obtained according to the signal intensity S received by the wireless sensing anchor node 2;
when the wireless sensing mobile node 4 transmits a wireless signal, the rocker arm strapdown inertial navigation device 3 records position information resolved by inertial navigation at the moment and sends the position information to the fuselage strapdown inertial navigation device 1; when the machine body wireless sensing anchor node 2 receives a wireless signal, the ranging time position information sent by the rocker arm strapdown inertial navigation device 3 is received, and the machine body position attitude information and the rocker arm position attitude information resolved by the machine body strapdown inertial navigation device 1 are recorded;
fifthly, utilizing the position and attitude information of the machine body, the position and attitude information of the rocker arm and the relative distance between the machine body and the rocker arm recorded at the time of transmitting the wireless ranging signal in the fourth step to carry out collaborative navigation filtering calculation based on a Kalman filter, and estimating the position, the speed and the attitude error of the rocker arm strapdown inertial navigation device 3;
1) the state variable of the collaborative navigation filter isWherein Δ PI=[ΔPNΔPUΔPE]TThe position error of the rocker arm strapdown inertial navigation device 3 is shown; Δ V ═ Δ VNΔVUΔVE]TRepresenting the speed error of the rocker arm strapdown inertial navigation device 3; phi is ═ phiNφUφE]TRepresenting the attitude error of the rocker arm strapdown inertial navigation device 3;
2) the collaborative navigation filtering state equation consists of a speed error equation, a position error equation and an attitude error equation of the rocker arm strapdown inertial navigation device 3:
the velocity error equation:
wherein the content of the first and second substances,
equation of position error
Equation of attitude error
Discretizing and rewriting equations (9), (10), (11) into a collaborative navigation filtering state equation form, wherein tkRepresenting a filtering time point;
X(tk)=F(tk-1)X(tk-1) (12)
3) the collaborative navigation filtering measurement equation is as follows:
4) correcting the synchronous error of the strapdown inertial device 1
The update rate of the positioning data of the body strapdown inertial navigation device 1 is higher than that of the positioning data of wireless distance measurementThe new rate can be calculated by adopting a linear interpolation method, when the rocker arm wireless sensing mobile node 4 transmits a wireless signal, the body strapdown inertial navigation device 1 records a time interval delta T, delta T is less than delta T, and the position P of the body strapdown inertial navigation device 1 for collaborative navigation is calculated by adopting the interpolation method1:
5) Calculated quantity measurement Z
At the moment of transmitting the wireless ranging signal, the position of the wireless sensing anchor node 2 of the machine body (namely the position of the strapdown inertial navigation device of the machine body) isThe position of a rocker arm wireless sensing mobile 4 node, namely the position of a rocker arm strapdown inertial navigation device 3 isAccording to the geometrical relationship:
wherein, Y represents the relative distance calculated by using the position of the rocker arm strapdown inertial navigation device 3 and the position of the fuselage strapdown inertial navigation device 1. The amount of co-navigation filtering, Z, is then calculated as follows:
Z=Y-d
6) the fuselage strapdown inertial navigation device 1 can obtain the estimated value of the collaborative navigation filtering state quantity by utilizing the well-known Kalman filtering algorithm
Sixthly, utilizing the collaborative navigation filtering state estimation valueCorrecting the position, speed and attitude errors of the rocker arm strapdown inertial navigation device 3 to obtain the position, speed and attitude of the rocker arm after error correction:
the body strapdown inertial navigation device 1 willSending the error to a rocker arm strapdown inertial navigation device 3, and carrying out closed-loop point correction on the error:
P2(tk)=P2(tk)-ΔP(tk) (17)
V2(tk)=V2(tk)-ΔV(tk) (18)
obtaining the position, speed and attitude matrix of the error corrected device 3, and correcting the errorAccording to step two, 6), replacing the variable superscript 1 with 2, and calculating the posture angle of the rocker arm after error correction: pitch angle theta2Heading angle psi2And roll angle gamma2。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010327977.XA CN111323011A (en) | 2020-04-23 | 2020-04-23 | Coal mining machine body and rocker arm cooperative positioning device and positioning method |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010327977.XA CN111323011A (en) | 2020-04-23 | 2020-04-23 | Coal mining machine body and rocker arm cooperative positioning device and positioning method |
Publications (1)
Publication Number | Publication Date |
---|---|
CN111323011A true CN111323011A (en) | 2020-06-23 |
Family
ID=71172000
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202010327977.XA Pending CN111323011A (en) | 2020-04-23 | 2020-04-23 | Coal mining machine body and rocker arm cooperative positioning device and positioning method |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN111323011A (en) |
Cited By (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN111894582A (en) * | 2020-08-04 | 2020-11-06 | 中国矿业大学 | Control method of coal mining machine |
CN112902952A (en) * | 2021-01-21 | 2021-06-04 | 中国煤炭科工集团太原研究院有限公司 | Development machine pose detection method based on millimeter wave radar and inertial navigation |
Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2010151459A (en) * | 2008-12-24 | 2010-07-08 | Seiko Epson Corp | Method and device for calculating position |
CN103413185A (en) * | 2013-08-07 | 2013-11-27 | 中国矿业大学 | Optimal arrangement method of coal mining machine rocker arm vibration sensors |
CN108387227A (en) * | 2018-02-22 | 2018-08-10 | 北京航空航天大学 | The multinode information fusion method and system of airborne distribution POS |
CN109974715A (en) * | 2019-04-30 | 2019-07-05 | 中国矿业大学(北京) | The development machine autonomous navigation system and method for inertial navigation and spot identification combination |
CN110118550A (en) * | 2019-05-14 | 2019-08-13 | 中国矿业大学 | A kind of generation system and method for fully-mechanized mining working high-precision three-dimensional navigation map |
CN110162036A (en) * | 2019-04-09 | 2019-08-23 | 中国矿业大学 | A kind of development machine Camera calibration system and method |
CN110702109A (en) * | 2019-06-05 | 2020-01-17 | 西京学院 | Coal mining machine inertial navigation/wireless sensor network combined positioning method |
-
2020
- 2020-04-23 CN CN202010327977.XA patent/CN111323011A/en active Pending
Patent Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2010151459A (en) * | 2008-12-24 | 2010-07-08 | Seiko Epson Corp | Method and device for calculating position |
CN103413185A (en) * | 2013-08-07 | 2013-11-27 | 中国矿业大学 | Optimal arrangement method of coal mining machine rocker arm vibration sensors |
CN108387227A (en) * | 2018-02-22 | 2018-08-10 | 北京航空航天大学 | The multinode information fusion method and system of airborne distribution POS |
CN110162036A (en) * | 2019-04-09 | 2019-08-23 | 中国矿业大学 | A kind of development machine Camera calibration system and method |
CN109974715A (en) * | 2019-04-30 | 2019-07-05 | 中国矿业大学(北京) | The development machine autonomous navigation system and method for inertial navigation and spot identification combination |
CN110118550A (en) * | 2019-05-14 | 2019-08-13 | 中国矿业大学 | A kind of generation system and method for fully-mechanized mining working high-precision three-dimensional navigation map |
CN110702109A (en) * | 2019-06-05 | 2020-01-17 | 西京学院 | Coal mining machine inertial navigation/wireless sensor network combined positioning method |
Non-Patent Citations (2)
Title |
---|
SCHIFFBAUER W H 等: ""Navigation and control of continuous mining systems for coal mining"", 《CONFERENCE RECORD OF THE 1996 IEEE INDUSTRY APPLICATIONS CONFERENCE THIRTY-FIRST IASANNUAL MEETING》 * |
巩剑波等: "采煤机姿态矫正系统研究综述", 《煤炭技术》 * |
Cited By (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN111894582A (en) * | 2020-08-04 | 2020-11-06 | 中国矿业大学 | Control method of coal mining machine |
CN111894582B (en) * | 2020-08-04 | 2021-09-24 | 中国矿业大学 | Control method of coal mining machine |
CN112902952A (en) * | 2021-01-21 | 2021-06-04 | 中国煤炭科工集团太原研究院有限公司 | Development machine pose detection method based on millimeter wave radar and inertial navigation |
CN112902952B (en) * | 2021-01-21 | 2023-11-10 | 中国煤炭科工集团太原研究院有限公司 | Heading machine pose detection method based on millimeter wave radar and inertial navigation |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN110702109B (en) | Coal mining machine inertial navigation/wireless sensor network combined positioning method | |
CN109813311B (en) | Unmanned aerial vehicle formation collaborative navigation method | |
JP4586172B2 (en) | Inertial navigation system | |
CN101718560B (en) | Strapdown system error inhibition method based on uniaxial four-position rotation and stop scheme | |
CN107656301A (en) | A kind of vehicle positioning method based on Multi-source Information Fusion | |
CN109540130A (en) | A kind of continuous milling machine inertial navigation positioning and orienting method | |
CN106507913B (en) | Combined positioning method for pipeline mapping | |
CN104937377B (en) | Method and apparatus for the vertical orientation for handling unconfined portable navigating device | |
CN112378399B (en) | Coal mine tunnel tunneling robot precise positioning and orientation method based on strapdown inertial navigation and digital total station | |
CN107289942B (en) | Relative navigation system and method for formation flight | |
CN110133692B (en) | Inertial navigation technology-assisted high-precision GNSS dynamic inclination measurement system and method | |
CN109916395A (en) | A kind of autonomous Fault-tolerant Integrated navigation algorithm of posture | |
CN111323011A (en) | Coal mining machine body and rocker arm cooperative positioning device and positioning method | |
CN109959374B (en) | Full-time and full-range reverse smooth filtering method for pedestrian inertial navigation | |
CN114739425A (en) | Coal mining machine positioning calibration system based on RTK-GNSS and total station and application method | |
CN110847905A (en) | Autonomous navigation system and method for coal mining machine | |
CN111207742B (en) | Coal mining machine positioning and attitude determining method with additional external orientation element constraint | |
CN108868772A (en) | A kind of continuous milling machine quickly collimates control method | |
CN105547289A (en) | Underwater vehicle combined navigation system and navigation information fusion method | |
CN114753841A (en) | Coal mining machine posture correction method and device | |
CN111207743B (en) | Method for realizing centimeter-level accurate positioning based on close coupling of encoder and inertial equipment | |
CN113075650A (en) | Underground roadway tunneling equipment real-time positioning method based on UWB and inertial unit | |
CN110411443A (en) | A kind of rocker arm of coal mining machine inertia/visual combination determines appearance device and method | |
CN112781588B (en) | Navigation resolving method for while-drilling gyroscope positioning and orientation instrument | |
CN113236363A (en) | Mining equipment navigation positioning method, system, equipment and readable storage medium |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination |