3) Taking the relative distance between the two inertial navigation systems after installation as a fixed value as a constraint condition, the filtering measurement equation is derived; The set value is r, which satisfies the following relationship:
Equation (1) can be abbreviated as ZK = h (xk)+vk, where subscript k represents k time; Quantitative measure z = [r]; V is the measurement noise; The equation is taken as the measurement equation of extended Kalman filter.
4) The filter outputs the north, east and sky position coordinates of the master inertial navigation and the slave inertial navigation, and transmits the north, east and sky position coordinates of the master inertial navigation to the data acquisition and display system as the positioning result of the shearer.
In step (1), the dead reckoning algorithm is as follows:
Where the subscripts k- 1 and k represent time k- 1 and k respectively; N, e and u are the position coordinates of the inertial navigation system in the north, east and sky directions respectively; V is the speed value output by the shaft encoder; T is the sampling period; θ is the heading angle and pitch angle of inertial navigation output, respectively.
According to the multi-inertial navigation positioning device and method for the shearer, two inertial navigations are taken as examples, and the constraint condition that the relative distance between the installed master inertial navigation and the slave inertial navigation is a fixed value is used, so that the low-cost slave inertial navigation is used to calibrate the medium-cost master inertial navigation to achieve high-precision positioning, which can meet the positioning accuracy requirements of the shearer in the underground coal mine and lay a foundation for realizing the intellectualization of the fully mechanized mining face.
Brief description of drawings
Figure 1 shows the multi-inertial navigation and positioning device of shearer.
Fig. 2 is the principle block diagram of the multi-inertial navigation and positioning method of the shearer.
In the figure: 1, main inertial navigation system; 2. For the solution system; 3. Used for slave inertial navigation; 4, for the shaft encoder; 5. It is a filter; 6, explosion-proof shell; 7. Data acquisition and display system; 8. Electrical control cabinet of shearer.
expand on
The invention will be further explained with reference to the attached drawings.
Example 1: As shown in figure 1, taking two inertial navigation systems as examples, the multi-inertial navigation and positioning device of the shearer comprises a master inertial navigation system 1, a slave inertial navigation system 3, a shaft encoder 4, a resolving system 2, a filter 5, a data acquisition and display system 7 and an explosion-proof shell 6.
The master inertial navigation system 1 is a medium-cost inertial navigation system, and the slave inertial navigation system 3 is a low-cost inertial navigation system.
The front end and the back end of the shearer are respectively fixed with an explosion-proof shell 6, in which the main inertial navigation system 1 and the calculation system 2 are installed, and in which the slave inertial navigation system 3 and the calculation system 2 are installed; The shaft encoder 4 is installed in the walking part of the shearer; The filter 5 is installed in the electrical control cabinet 8 of the shearer; The data acquisition and display system 7 is installed on the industrial computer in the gateway.
As shown in fig. 2, the multi-inertial navigation and positioning method of the shearer of the present invention includes the following steps:
1) Taking two inertial navigation systems as examples, the solution system 2 collects the attitude angle of the inertial navigation system and the speed data of the shaft angle encoder 4, and uses the dead reckoning algorithm to solve the position coordinates. The solution system transmits the position coordinates (pMaster and pSlave) of the master inertial navigation system 1 and the slave inertial navigation system 3 to the filter 5.
2) The state equation of filter 5 is deduced by dead reckoning algorithm. According to the dead reckoning algorithm, the positioning equation can be abbreviated as where the subscripts k- 1 and k represent k- 1 time and k time respectively; State quantity x = [n 1 e 1 u 1 n2e2u2] t, n 1, e1,u1is the north, east and east of the main inertial navigation system1 V is the speed value output by the shaft encoder 4; T is the sampling period; θ is the heading angle and pitch angle of inertial navigation output; W is state noise. This positioning equation is used as the state equation of the extended Kalman filter 5.
3) Under the constraint that the relative distance between the master inertial navigation system 1 and the slave inertial navigation system 3 is fixed, the measurement equation of the filter 5 is derived. The set value is r, which satisfies the following relationship:
Equation (1) can be abbreviated as ZK = h (xk)+vk, where subscript k represents k time; z =[r]； V is the measurement noise. This equation is taken as the measurement equation of the extended Kalman filter 5.
4) The filter 5 outputs the position coordinates of the master inertial navigation 1 and the slave inertial navigation 3, and transmits the position coordinates of the master inertial navigation 1 to the data acquisition and display system 7 as the positioning result of the shearer.
In step (1), the dead reckoning algorithm is as follows:
Where the subscripts k- 1 and k represent time k- 1 and k respectively; N, e and u are the position coordinates of the inertial navigation system in the north, east and sky directions respectively; V is the speed value output by the shaft encoder 4; T is the sampling period; θ is the heading angle and pitch angle of inertial navigation output, respectively.
This patent takes two inertial navigation systems as examples to illustrate the multi-inertial navigation and positioning device and method of shearer, which is also applicable to three or more multi-inertial navigation and positioning systems.
The above is only the preferred embodiment of the present invention, and it should be pointed out that those skilled in the art can make various improvements and modifications without departing from the principle of the present invention, and these improvements and modifications should also be regarded as the protection scope of the present invention.