Disclosure of Invention
Aiming at the defect that the position of a coal mining machine cannot be effectively monitored in the prior art, the invention provides an autonomous positioning method of the coal mining machine based on inertial navigation data, which comprises the following steps:
according to an inertial sensor installed on a coal mining machine, first positioning data of the coal mining machine is obtained through measurement under a carrier coordinate system, wherein the first positioning data comprises the following steps: the method comprises the following steps of (1) enabling a coal mining machine to perform translational acceleration under a carrier coordinate system and enabling a coal mining machine to perform rotational velocity under the carrier coordinate system;
constructing a first attitude matrix under a carrier coordinate system, and differentiating to obtain a second attitude matrix under a navigation coordinate system;
performing integral calculation on the translational acceleration value in the first positioning data by combining with the initial estimation value of the speed and the position of the coal mining machine to obtain second positioning data under a navigation coordinate system;
performing inverse operation according to the second attitude matrix to obtain an attitude angle of the coal mining machine under a navigation coordinate system;
and determining the position of the coal mining machine under the navigation coordinate system according to the second positioning data and the attitude angle.
Further, the constructing the first attitude matrix in the carrier coordinate system includes:
constructing a first attitude matrix through an initial state course angle, a pitch angle and a roll angle of the coal mining machine, and constructing the first attitude matrix through the following formula:whereinIndicates the rotation angle between n and b,setting u as a rotation vector in the n system, rotating the carrier around u by β degrees with the rotation angles of l, m and n as the projection of u in the three rotation shaft directions in the n system;
said differentiating the first attitude matrix comprises: the product of the first attitude matrix and the rotational angular velocity value in the carrier coordinate system is taken as the result of the differentiation.
Further, the second positioning data comprises: the speed value of the coal mining machine under the navigation coordinate system and the displacement value of the coal mining machine under the carrier coordinate system.
Further, the inertial sensor includes: a pendulum accelerometer for measuring a translational acceleration value of the first positioning data and a laser gyroscope for measuring a rotational angular velocity value of the first positioning data.
Further, before the integrating the translational acceleration value in the first positioning data with the initial estimation value of the speed and the position of the shearer to obtain second positioning data in a navigation coordinate system, the method further comprises:
and carrying out error compensation on the rotation angular velocity and the translational acceleration of the first positioning data by utilizing extended Kalman filtering.
And further, updating the elements in the first attitude matrix by taking the direction cosine elements in the second attitude matrix obtained after differentiation as the elements in the matrix, and performing integral calculation by adopting the updated first attitude matrix and combining the initial estimated values of the speed and the position of the coal mining machine to obtain second positioning data under a navigation coordinate system.
In a second aspect, the invention provides an autonomous positioning device for a coal mining machine based on inertial navigation data, comprising:
the measurement module is used for measuring first positioning data of the coal mining machine under a carrier coordinate system according to an inertial sensor installed on the coal mining machine, wherein the first positioning data comprises: the method comprises the following steps of (1) enabling a coal mining machine to perform translational acceleration under a carrier coordinate system and enabling a coal mining machine to perform rotational velocity under the carrier coordinate system;
the integral operation module is used for carrying out integral calculation on the translational acceleration value in the first positioning data by combining with the initial estimation value of the speed and the position of the coal mining machine to obtain second positioning data under a navigation coordinate system;
the differential operation module is used for constructing a first attitude matrix under a carrier coordinate system and differentiating to obtain a second attitude matrix under a navigation coordinate system;
the inverse operation module is used for carrying out inverse operation according to the second attitude matrix to obtain an attitude angle of the coal mining machine under a navigation coordinate system;
and the position determining module is used for determining the position of the coal mining machine under the navigation coordinate system according to the second positioning data and the attitude angle.
Further, the constructing the first attitude matrix in the differential operation module under the carrier coordinate system includes:
constructing a first attitude matrix through an initial state course angle, a pitch angle and a roll angle of the coal mining machine, and constructing the first attitude matrix through the following formula:whereinIndicates the rotation angle between n and b,respectively represent n andsetting u as a rotation vector in an n system, rotating the carrier around u, and respectively representing the projection of u in three rotating shaft directions in the n system by using the rotation angles of β, l, m and n;
said differentiating the first attitude matrix comprises: the product of the first attitude matrix and the rotational angular velocity value in the carrier coordinate system is taken as the result of the differentiation.
Further, the second positioning data comprises: the speed value of the coal mining machine under the navigation coordinate system and the displacement value of the coal mining machine under the carrier coordinate system.
Further, the inertial sensor includes: a pendulum accelerometer for measuring a translational acceleration value of the first positioning data and a laser gyroscope for measuring a rotational angular velocity value of the first positioning data.
The invention has the beneficial effects that:
the product is dedicated to the field of intelligent unmanned mining of coal mines, the key device technology is attacked, the actual factors of a production field are deeply considered on the basis of the automatic control of fully-mechanized mining equipment, the military inertial navigation key technology is introduced, the actual demand characteristics of coal mine application are combined, the inertial device, the system integration and the software development are combined, the autonomous intellectual property right is possessed, and the localization of the positioning equipment of the coal mining machine is realized. The intelligent fully-mechanized coal mining operation self-guiding control module is matched with an intelligent fully-mechanized coal mining operation self-guiding control module which is designed by independent research and development and has the core technology of automatic positioning navigation and automatic working face straightening, so that the self-guiding coal mining of the intelligent fully-mechanized coal mining operation of the coal mine is realized.
According to the invention, the trial-manufacture inertial navigation device and the matched system are independently researched and developed for the first time, so that the localization of core key equipment and technology is realized, the dependence of foreign core key technology is reduced, and the goal of 100% localization rate of the whole system and device is realized;
the invention adopts the high-precision inertial navigation technology in the field of Beidou navigation and positioning for the first time and independently develops and develops a high-precision combined positioning algorithm;
the invention adopts a data real-time fusion processing technology and an embedded system integration technology, can synchronize, acquire and transmit the data of a high-precision inertial navigation system and a mileage meter, draws accurate three-dimensional space coordinate information after comprehensively processing the information, and realizes the three-dimensional visual presentation of the information data of the coal face.
Detailed Description
In the following description, for purposes of explanation and not limitation, specific details are set forth such as particular equipment structures, interfaces, techniques, etc. in order to provide a thorough understanding of the present invention. It will be apparent, however, to one skilled in the art that the present invention may be practiced in other embodiments that depart from these specific details. In other instances, detailed descriptions of well-known devices, circuits, and methods are omitted so as not to obscure the description of the present invention with unnecessary detail.
As shown in fig. 1, the invention discloses an autonomous positioning method for a coal mining machine, which comprises the following steps:
s1: according to an inertial sensor installed on a coal mining machine, first positioning data of the coal mining machine is obtained through measurement under a carrier coordinate system, wherein the first positioning data comprises the following steps: the method comprises the following steps of (1) enabling a coal mining machine to perform translational acceleration under a carrier coordinate system and enabling a coal mining machine to perform rotational velocity under the carrier coordinate system;
s2: constructing a first attitude matrix under a carrier coordinate system, and differentiating to obtain a second attitude matrix under a navigation coordinate system;
s3: performing integral calculation on the translational acceleration value in the first positioning data by combining with the initial estimation value of the speed and the position of the coal mining machine to obtain second positioning data under a navigation coordinate system;
s4: performing inverse operation according to the second attitude matrix to obtain an attitude angle of the coal mining machine under a navigation coordinate system;
s5: and determining the position of the coal mining machine under the navigation coordinate system according to the second positioning data and the attitude angle.
As shown in fig. 2, a schematic diagram of autonomous positioning of a shearer of the present invention is shown.
In step S1, the acceleration of the shearer in the three directions of the working plane can be measured through the accelerometer. The acceleration original value measured by the accelerometer under the carrier coordinate system is ab(t)＝(abx(t),aby(t),abz(t))TWherein a isbx(t),aby(t),abz(t) are acceleration values of the accelerometer in three directions of an x axis, a axis and a z axis of a carrier coordinate system (b system) respectively, and abAnd (t) is the transposed column vector.
The original value of the rotation angular velocity measured by the gyroscope through the angular velocities around three directional axes of the coal mining machine is omegab(t)＝(ωbx(t),ωby(t),ωbz(t)), wherein ωbx(t),ωby(t),ωbzAnd (t) is the angular speed of the gyroscope in three directions of an x axis, a y axis and a z axis of a carrier coordinate system (system b), wherein t is a time unit.
Omega obtained by measuring at current moment when angular velocity of three-direction shaft of coal mining machine is representedb(t) the platform rotation rate called in the subsequent updating calculation process of the attitude reference of the coal mining machine is omega of the lag timeb(t)。
In an embodiment, the navigation accelerometer and the laser gyro are directly mounted on the carrier. And subtracting the calculated angular velocity of the navigation coordinate system relative to the inertial space from the angular velocity information measured by the laser gyroscope to obtain the angular velocity of the carrier coordinate system relative to the navigation coordinate system, and calculating the attitude matrix by using the information. The axial acceleration information of the carrier coordinate system can be converted into the axial direction of the navigation coordinate system, and then navigation calculation is carried out. And extracting attitude and heading information by using the attitude matrix elements. The functions of calculating navigation data of the navigation platform can be replaced by the functions of posture matrix calculation, coordinate transformation of acceleration information and posture and course angle calculation. And calculating the angular speed information of the navigation coordinate system relative to the gyroscope moment information on the platform coordinate system.
In some illustrative embodiments, the inertial sensor includes: a pendulum accelerometer for measuring a translational acceleration value of the first positioning data and a laser gyroscope for measuring a rotational angular velocity value of the first positioning data.
As shown in fig. 3, a schematic block diagram of a laser gyro used in the present invention is shown.
The laser gyroscope obtains the optical path angular velocity omega by measuring the frequency difference of two beams of clockwise and anticlockwise light in the rotary closed optical path and calculating the frequency difference. In the closed optical path, two beams of light transmitted in the clockwise direction and the counterclockwise direction emitted by the same light source interfere with each other, and the angular velocity ω of rotation of the closed optical path can be measured by detecting the phase difference or the change of the interference fringes. The basic components of a laser gyro are a ring laser, which consists of a closed optical path made of triangular or square quartz, in which one or several tubes containing a gas mixture (helium-neon gas), two opaque mirrors and a semi-transparent mirror are arranged. The mixed gas is excited by a high-frequency power supply or a direct-current power supply to generate monochromatic laser. To maintain the loop resonance, the perimeter of the loop should be an integer multiple of the wavelength of the light wave. The semi-transparent mirror is used to lead the laser out of the loop, two beams of laser which are transmitted oppositely are interfered by the reflector, and a digital angular speed signal which is proportional to the output angle is input through the photoelectric detector and the circuit.
The laser gyro adopts three reflectors to form an annular resonant cavity, namely a closed loop. Photons propagating along the optical axis of the laser tube are emitted to two sides through the lenses M4 and M5 and then respectively emitted from the lenses M4 and M51→M2→M3And M3→M2→M1And two beams with opposite propagation directions are formed in the loop after being reflected from the other end. For each light beam, only photons with the phase difference of 2 pi integral multiple can induce and emit corresponding second-generation photons when returning to the original position through one circle, the photons with the phase difference of not more than 2 pi integral multiple gradually attenuate until disappear according to the law, the enhanced photons are more than the attenuated photons, and the closed light path works in a resonance state.
As shown in fig. 4, a circular light path of the laser gyro of the present invention is shown.
As shown in FIG. 5, a functional block diagram of the pendulum force-balanced accelerometer of the present invention is shown.
The accelerometer is one of the important elements in an inertial measurement unit to measure the constant and low frequency accelerations acting along its input axis. Accelerometers utilize the negative feedback principle of a closed-loop system to levitate a proof mass at a fixed position in their structure. The accelerometer is composed of the following five parts:
① inertial mass whose inertia generates a force when accelerated, ② elastic hinge, ③ sensor for inertial force work, ④ force generator, ⑤ electronic amplifier, all of which are placed in a closed housing.
When acceleration is applied along the sensitive axis of the sensor ③ which applies work from the sensitive inertial force, the position of the proof mass changes, the position detector detects this change and then inputs a signal to the amplifier which drives the force generator to return the proof mass to zero.
As shown in fig. 6, a diagram of the electromechanical components of the pendulum accelerometer of the present invention is shown. The labels 1 and 3 in the figure are respectively a fixed wiring block and a movable wiring block; 2-a soft wire band; 4-a bearing support; 5-a float; 6-torque coil; 7-a sleeve; 8, 9-torquer and magnetizer; 10-a pendulum bob; 11-jewel bearings; 12, 13-rotor and stator of the angle sensor, respectively.
As shown in fig. 7, a schematic view of the pendulum accelerometer of the present invention is shown.
In the figure, 7-1 is a flexible support, 7-2 is a shell, 7-3 is a yoke, 7-4 is a torquer moving coil, 7-5 is a permanent magnet, 7-6 is a swing part, 7-7 is a signaler exciting coil, 7-8 is a signaler moving coil, and 7-9 is an amplifier.
The accelerometer consists of a pendulum assembly, a signal sensor, a push-pull permanent magnet torquer and a flexible rod, wherein silicone oil is filled in the accelerometer to serve as damping liquid, and a precise temperature control device is arranged to ensure that the accelerometer works at a constant working temperature. One end of the flexible rod is fixed on the watch case, the other end is adhered with the annunciator moving coil to form a cantilever beam, the moment device moving coil is fixed on the swinging sheet, one end of the permanent magnet is fixed on the watch case, and the other end is sleeved in the moment device moving coil in a hollow mode. When the meter case (base) has an acceleration a along the direction of the IA axis, the flexible rod rotates around the center of the flexible joint under the action of inertia moment to generate an output angle theta 0, the annunciator detects the theta 0 and converts the theta 0 into a voltage signal, the voltage signal is amplified and then added into a moving coil of the torquer, the generated push-pull moment counteracts the inertia moment to return the theta 0 to zero, then the current in the balancing loop is converted into an output voltage through the sampling resistor, and the acceleration a can be extracted from the output voltage.
In steps S2 and S3, there is no chronological order of steps, and in an embodiment, step S3 may be executed after step S2 is executed first, or step S3 may be executed first, and then step S2 is executed.
For example, in step S2, a first attitude matrix is constructed in the carrier coordinate system, and during the construction, the heading angle, the pitch angle and the roll angle of the shearer in different coordinate systems need to be determined. And carrying out corresponding coordinate transformation through an inertial navigation device to obtain a corresponding angle.
1) Coordinate transformation
Navigation information such as the attitude, the speed, the position and the like of a carrier obtained by the inertial navigation device through calculation is required to be in a certain coordinate system, and different coordinate systems can be selected by different navigation systems according to different navigation requirements. The attitude of the shearer is mainly represented by a pitch angle, a roll angle and a course angle, as shown in fig. 8.
The included angle between the coal mining machine and the due north direction in the working face direction represents the yaw angle of the coal mining machine, the included angle between the coal mining machine and the local horizontal plane in the working face direction represents the pitch angle of the coal mining machine, and the included angle between the coal mining machine and the local horizontal plane in the working face advancing direction represents the roll angle of the coal mining machine. The inertial navigation technology measures the acceleration of the coal mining machine by means of an inertial sensor mounted on the coal mining machine and performs two times of integral calculation, so as to obtain the position state of the coal mining machine. Because the sensor is fixedly connected with the coal mining machine, the information of the coal mining machine directly obtained through integral operation is position information under a carrier coordinate system, and in order to unify the information of the whole fully mechanized mining face, coordinate system transformation is required to transform the position information of the coal mining machine from the carrier coordinate system to a navigation coordinate system. Let the heading angle of the carrier be ψ (conventionally, north is the positive), the pitch angle be θ, the roll angle be γ, take the geographic coordinate system g as the navigation coordinate system, select the carrier coordinate system as the right front upper coordinate system, the navigation coordinate system as the east-north-sky direction, and their transformation relationship is shown in fig. 9.
As described above with reference to fig. 10, the transformation process between the carrier coordinate system (b system) and the navigation coordinate system (n system) is described.
2) Gesture description
The inertial navigation device utilizes a laser gyroscope and an accelerometer to construct an Inertial Measurement Unit (IMU), the Inertial Measurement Unit (IMU) is fixedly installed on a carrier of the coal mining machine, so that the laser gyroscope and the accelerometer measure physical quantities based on a carrier coordinate system, namely the laser gyroscope measures the three-axis rotation angular velocity of the coal mining machine based on the carrier coordinate system, and the accelerometer measures the acceleration of the three-axis translational motion of the coal mining machine based on the carrier coordinate system. The navigation calculation of the inertial navigation device is based on the navigation coordinate system, so the coordinate transformation process in 1) needs to be completed. In a three-dimensional space, the pose transformation of the coal mining machine in the moving process can be represented by a group of rotation matrixes and translation vectors. The translation vector represents a vector from the original point of the navigation coordinate system to the original point of the carrier coordinate system, and the rotation matrix is also called an attitude matrix and represents the attitude of the coal mining machine body in real time.
In some demonstrative embodiments, the constructing of the first pose matrix in the carrier coordinate system in step S3 may include:
constructing a first course angle, a pitch angle and a roll angle of the coal mining machineAn attitude matrix, a first attitude matrix is constructed by the following formula:whereinIndicates the rotation angle between n and b,and u is a rotation vector in the n system, the carrier rotates around u, and the rotation angles are β, l, m and n respectively represent the projection of u on the three rotating shaft directions in the n system.
The quaternion is a hypercomplex number consisting of a real number and three imaginary number units, wherein the real number represents a rotation angle, the imaginary number unit represents a rotating shaft direction, u is a rotation vector in an n system, a carrier rotates around u, the rotation angle is β, and the projection u of u in the n system is un(l, m, n), l, m, n are respectively expressed as the projection of u in the three rotation axis directions in the n system, and then Q (e) is used0,e1,e2,e3)＝e0+e1i+e2j+e3kThe attitude matrix of the constructed quaternion representation is:
whereinIndicates the rotation angle between n and b,respectively represents the rotating shaft direction under a three-dimensional coordinate system between the n system and the b system.
The relationship between the formula (1) and 1) the rotation angle is expressed as shown in the formula (2):
wherein psi represents a heading angle of the carrier equivalent rotation between the b system and the n system, theta represents a pitch angle of the carrier equivalent rotation between the b system and the n system, and gamma represents a roll angle of the carrier equivalent rotation between the b system and the n system. (2) Is a direction cosine element.
In step S2, differentiating the first posture matrix to obtain a second posture matrix in the navigation coordinate system; the differentiating the first attitude matrix includes: the product of the first attitude matrix and the rotational angular velocity value in the carrier coordinate system is taken as the result of the differentiation.
The navigation computer performs differential updating on the attitude under the navigation coordinate system, namely the differential equation is as follows:
taking the result of the above differential equationAs a second attitude matrix. The initial estimation value of the attitude of the coal mining machine comprises an initial state course angle, a pitch angle and a roll angle, a first attitude matrix constructed by adopting the initial estimation value is used as a matrix R, and the angular speed omega of the coal mining machine around three-direction axes isb(t), the attitude reference updating calculation in fig. 2 is realized by the formula (5), and the obtained result is a second attitude matrix
The sign of the differential in the differential equation can be found in the lesson thought of the author Qinyuan in the textbook inertial navigation or the translator in the textbook GNSS and inertial and multi-sensor integrated navigation system principle, and represents the differential, and the same sign also represents the differential hereinafter.
Wherein,representing the attitude matrix of the carrier coordinate system relative to the navigation coordinate system,the angular velocity of the carrier coordinate system relative to the navigation coordinate system is expressed, and the formula (5) is transformed to obtain:
wherein,representing the attitude matrix of the carrier coordinate system b relative to the navigation coordinate system n,representing the angular velocity of the carrier coordinate system relative to the navigation coordinate system,and respectively representing the projection of the rotation angular speed of the navigation coordinate system and the carrier coordinate system relative to the inertial coordinate system i in the navigation coordinate system and the carrier coordinate system.Representing the carrier coordinate system with respect to inertiaThe projection of the angular velocity of the rotation of the coordinate system in the navigation coordinate system,representing the attitude matrix of the navigational coordinate system relative to the carrier coordinate system.
Therefore, the formula for posture update is:
wherein,andthe attitude matrices at m-1 and m moments, respectively, i denotes the inertial coordinate system, b denotes the carrier coordinate system, n denotes the navigation coordinate system, and m-1 denote moments and are time units, respectively.
In the process of the above-described differential calculation,the following steps are performed to obtain the positioning and attitude determination calculation of the inertial navigation device based on the navigation coordinate system (n system), so the above coordinate transformation needs to be completed. Generating an attitude matrix R as shown in formula (1), namely, the expressions of the gravity acceleration and the earth rotation angular velocity in the formula (3) in a navigation coordinate system are as follows:
wherein, gn,ωnRespectively representing gravity acceleration and earth rotation angular velocity under a navigation coordinate system (n system); t is transposition.
gb，ωbRespectively representing the gravity acceleration and the earth rotation angular velocity under a carrier coordinate system (b system).
The process of constructing the first posture matrix and differentiating to obtain the second posture matrix in the above detailed S3 is described in detail below, where the process of obtaining the second positioning data in the navigation coordinate system in step S2 includes: and performing integral calculation on the translational acceleration value in the first positioning data by combining the initial estimation value of the speed and the position of the coal mining machine.
The initial position of the coal mining machine under the carrier coordinate system comprises the following steps: an initial value of velocity, and an initial value of position, and are respectively denoted by vg(0)、pg(0) Wherein the components of the gravitational acceleration and the earth rotation angular velocity vector in the geographical coordinate system (initial alignment reference frame, navigation frame) are accurately known. The following were used:
gn＝[0 0 -g]T
ωn＝[0 ωecos L ωesin L]T (3)
l, g and ω, among otherseThe corresponding representation is local latitude, gravity acceleration magnitude and earth rotation angular rate magnitude.
The integration calculation process in S3 includes:
the navigation computer updates the real-time value of the attitude matrix R, namely the attitude matrix described by the formula (7), the acceleration information measured by the acceleration is based on the carrier coordinate system, and the acceleration a under the geographic coordinate system is obtained by subtracting the gravity acceleration ggLet the accelerometer measure a ═ ax ay az]TThe acceleration is integrated once to obtain the velocity VgIntegrating the velocity to obtain the displacement PgTherefore, the acceleration, velocity, displacement relationship is as follows:
ag＝R-1a-g
discretizing the inertial navigation element within the period T, namely equivalent to the formula:
Vgm＝Vgm-1+agm-1T
wherein, Vgm，Vgm-1Respectively representing the speed, P, at the m-time and the m-1 time in a geographic coordinate systemgm,Pgm-1Respectively representing the displacement in the geographic coordinate system at the time m and the time m-1, agm-1Representing the acceleration in the geographic coordinate system at time m-1.
In some demonstrative embodiments, the second positioning data may include: velocity value v of coal mining machine under navigation coordinate systemg(t) and displacement value p of coal mining machine in carrier coordinate systemg(t)。
Coordinate transformation is carried out on the carrier coordinate system to a navigation coordinate system, so the transformed acceleration an(t), velocity vg(t), displacement value pg(t) the following:
the integral calculation is the navigation calculation in fig. 2, the initial estimation of the direction cosine matrix and the shearer velocity and position in equation (1), the navigation calculation is performed, and the final calculation result is the shearer displacement p in equation (10)g(t) and shearer velocity vg(t)。
Therefore, the coal mining machine can perform differential updating on the angular rate of the gyroscope through the self inertial navigation system device to obtain the attitude of the coal mining machine, and perform integration on the real-time acceleration to obtain the speed and the displacement, so that the autonomous attitude-determining and positioning of the coal mining machine are realized.
In the above embodiment, the process of step S2 is explained, and the processes of step S4 and step S5 are explained below.
Step S4: performing inverse operation according to the second attitude matrix to obtain an attitude angle of the coal mining machine under a navigation coordinate system;
firstly, a course angle, a pitch angle and a roll angle of the coal mining machine are obtained through initial measurement, a first attitude matrix of the coal mining machine is expressed according to an initial estimation value of the attitude of the coal mining machine, and a second attitude matrix after operation is obtained through differential operation in a formula (5)And performing inverse operation on the second attitude matrix, and acquiring a course angle, a pitch angle and a roll angle of the coal mining machine under the navigation coordinate system as attitude angles of the coal mining machine under the navigation coordinate system. That is, after the inverse operation of direction cosine element (navigation calculation in fig. 2) is adopted in fig. 2, the course angle, pitch angle and roll angle of the coal mining machine under the navigation coordinate system are obtained again asAttitude data for the shearer in fig. 2.
Step S5: and determining the position of the coal mining machine under the navigation coordinate system according to the second positioning data and the attitude angle.
Since the second positioning data includes: in the formula (10), the velocity value of the shearer under the navigation coordinate system and the displacement value of the shearer under the carrier coordinate system also have the heading angle, the pitch angle and the roll angle obtained by inverse operation in the step S4. And obtaining the position of the coal mining machine after the displacement under the n series, wherein the position is the position after the inertial navigation.
In some illustrative embodiments, the direction cosine element in the second attitude matrix obtained after differentiation is used as an element in the matrix to update the element in the first attitude matrix, and the updated first attitude matrix is used in combination with the initial estimation values of the speed and the position of the coal mining machine to perform integral calculation to obtain second positioning data in a navigation coordinate system. Thereby obtaining the displacement p of the coal mining machine again after the integral operation of the navigation calculation againg(t) and shearer velocity vg(t)。
In some illustrative embodiments, before the integrating the translational acceleration value in the first positioning data with the initial estimate of the shearer's speed and position to obtain second positioning data in a navigation coordinate system, the method further comprises:
and carrying out error compensation on the rotation angular velocity and the translational acceleration of the first positioning data by utilizing extended Kalman filtering. The specific principle is shown in fig. 11.
Setting an initial value of an extended Kalman filtering algorithm, and only predicting and not executing an updating process by a filter when a static state of a coal mining machine is not detected;
if the coal mining machine is detected to be in a static state during static detection, as the data acquisition module acquires angular velocity and acceleration data and completes partial work of coordinate system transformation in the formula (1), the relative zero state correction process comprises the following steps: the initial speed obtained by resolving the formula (9) is used as an error observation value of the speed through zero speed correction, and an extended Kalman (Kalman) filtering algorithm is controlled to perform an updating process;
discretizing the inertial navigation element within the period T, namely equivalent to the formula:
Vgm＝Vgm-1+agm-1T
wherein, Vgm，Vgm-1Respectively representing the speed, P, of the coal mining machine under the geographic coordinate systems of the m moment and the m-1 momentgm,Pgm-1Respectively representing the displacement of the coal mining machine under the geographic coordinate systems of m time and m-1 time, agm-1Representing the acceleration of the shearer in the geographic coordinate system at the moment m-1.
The formula (9) is adopted to carry out discretization processing on the inertial navigation element in the period T, so that the effect of convenient calculation can be realized.
The initial velocity obtained by solving the formula (9) through zero velocity correction as an error observed value of the velocity means that the initial velocity v is taken as the velocityg(0) The initial velocity obtained by calculation is substituted into equation (9) as an error observed value of the velocity.
The acceleration output through the gyroscope and accelerometer in FIG. 11 is a mentioned aboveb(t) angular velocity is ω mentioned hereinbeforeb(t)。
And performing inertial navigation positioning calculation, namely performing integral calculation on the translational acceleration value in the first positioning data in combination with the initial estimation value of the speed and the position of the coal mining machine to obtain second positioning data in a navigation coordinate system, constructing a first attitude matrix in a carrier coordinate system, and performing differentiation to obtain a second attitude matrix in the navigation coordinate system.
The velocity resolved by inertial navigation positioning is v as mentioned aboveg(t) position is p as mentioned hereinbeforegAnd (t), the direction angle is a heading angle, a pitch angle and a roll angle after differential updating.
In the process of the correction feedback in fig. 11, the measured error values of the speed, the position, the direction angle, and the like are fed back to the current inertial navigation positioning calculation unit and are used as the position of the calculated speed, position, and direction angle values that are not corrected, and the process of the correction feedback can be understood as a process of obtaining accurate pose information by subtracting the error values from the values of the uncorrected speed, position, and direction angle. The error is an error value obtained through extended kalman filtering, i.e., the system error in fig. 11.
The pose information finally obtained in fig. 11 is the values of the speed, position, and direction angle after error correction.
In the error estimation process shown in fig. 11, the current inertial navigation resolving speed is used as an actual measurement value of the extended kalman filter, and then, according to whether the current coal mining machine is in a stationary state, a prediction process or an update process executed by the extended kalman filter algorithm is determined.
If the extended kalman filtering in fig. 11 is not performed, the method for solving the updated pose information of the coal mining machine of the present invention can be implemented, and only the influence of the system error continuously accumulated in the inertial navigation in the positioning process on the positioning accuracy can be solved to a certain extent by using the extended kalman filtering, so that the positioning accuracy is improved.
Performing optimal estimation on the errors of the acceleration, the angular velocity, the speed and the position in the system state vector by an extended Kalman filtering algorithm through an updating process, and feeding the errors back to a navigation positioning attitude determination resolving unit; the system state vector is a vector including error information of acceleration, angular velocity, velocity and the like.
And the inertial measurement data comprise velocity errors, position errors and attitude angle return to zero in the state vector of the compensated extended Kalman filtering algorithm, and the acceleration errors and the angular velocity errors are reserved.
The method comprises the steps of automatically acquiring the positioning data of the acceleration and the angular velocity, carrying out coordinate conversion, real-time compensation and deviation rectification and the like on the positioning data, and outputting the inertial navigation positioning data under a navigation coordinate system according to the inertial navigation deviation rectification error parameter, so that the autonomous attitude-fixing positioning method based on the inertial navigation not only can realize the positioning under the navigation coordinate system, but also solves the problem that the positioning cannot be carried out under the condition that electromagnetic wave positioning coverage signals are weak or blind areas exist, and the real-time deviation rectification provided by the invention solves the influence of system errors continuously accumulated in the positioning process of the inertial navigation on the positioning precision, thereby improving the positioning precision.
As shown in fig. 12, the present invention also discloses an autonomous positioning device for a coal mining machine based on inertial navigation data, the device comprising:
the measurement module 100 is configured to obtain first positioning data of the coal mining machine by measuring in a carrier coordinate system according to an inertial sensor installed on the coal mining machine, where the first positioning data includes: the method comprises the following steps of (1) enabling a coal mining machine to perform translational acceleration under a carrier coordinate system and enabling a coal mining machine to perform rotational velocity under the carrier coordinate system;
the differential operation module 200 is configured to construct a first attitude matrix in a carrier coordinate system, and perform differentiation to obtain a second attitude matrix in a navigation coordinate system;
the integral operation module 300 is configured to perform integral calculation on the translational acceleration value in the first positioning data in combination with an initial estimation value of the speed and the position of the coal mining machine to obtain second positioning data in a navigation coordinate system;
the inverse operation module 400 is used for performing inverse operation according to the second attitude matrix to obtain an attitude angle of the coal mining machine under a navigation coordinate system;
and the position determining module 500 is configured to determine the position of the shearer in the navigation coordinate system according to the second positioning data and the attitude angle.
The invention has the beneficial effects that:
the product is dedicated to the field of intelligent unmanned mining of coal mines, the key device technology is attacked, the actual factors of a production field are deeply considered on the basis of the automatic control of fully-mechanized mining equipment, the military inertial navigation key technology is introduced, the actual demand characteristics of coal mine application are combined, the inertial device, the system integration and the software development are combined, the autonomous intellectual property right is possessed, and the localization of the positioning equipment of the coal mining machine is realized. The intelligent fully-mechanized coal mining operation self-guiding control module is matched with an intelligent fully-mechanized coal mining operation self-guiding control module which is designed by independent research and development and has the core technology of automatic positioning navigation and automatic working face straightening, so that the self-guiding coal mining of the intelligent fully-mechanized coal mining operation of the coal mine is realized.
According to the invention, the trial-manufacture inertial navigation device and the matched system are independently researched and developed for the first time, so that the localization of core key equipment and technology is realized, the dependence of foreign core key technology is reduced, and the goal of 100% localization rate of the whole system and device is realized;
the invention adopts the high-precision inertial navigation technology in the field of Beidou navigation and positioning for the first time and independently develops and develops a high-precision combined positioning algorithm;
the invention adopts a data real-time fusion processing technology and an embedded system integration technology, can synchronize, acquire and transmit the data of a high-precision inertial navigation system and a mileage meter, draws accurate three-dimensional space coordinate information after comprehensively processing the information, and realizes the three-dimensional visual presentation of the information data of the coal face.
It is clear to those skilled in the art that, for convenience and brevity of description, the specific working processes of the above-described systems, apparatuses and units may refer to the corresponding processes in the foregoing method embodiments, and are not described herein again.
In the several embodiments provided in the present application, it should be understood that the disclosed system, apparatus and method may be implemented in other manners. For example, the above-described apparatus embodiments are merely illustrative, and for example, the division of the units is only one logical division, and other divisions may be realized in practice, for example, a plurality of units or components may be combined or integrated into another system, or some features may be omitted, or not executed. In addition, the shown or discussed mutual coupling or direct coupling or communication connection may be an indirect coupling or communication connection through some interfaces, devices or units, and may be in an electrical, mechanical or other form.
The units described as separate parts may or may not be physically separate, and parts displayed as units may or may not be physical units, may be located in one place, or may be distributed on a plurality of network units. Some or all of the units can be selected according to actual needs to achieve the purpose of the solution of the embodiment.
In addition, functional units in the embodiments of the present application may be integrated into one processing unit, or each unit may exist alone physically, or two or more units are integrated into one unit. The integrated unit can be realized in a form of hardware, and can also be realized in a form of a software functional unit.
The integrated unit, if implemented in the form of a software functional unit and sold or used as a stand-alone product, may be stored in a computer readable storage medium. Based on such understanding, the technical solution of the present application may be substantially implemented or contributed to by the prior art, or all or part of the technical solution may be embodied in a software product, which is stored in a storage medium and includes instructions for causing a computer device (which may be a personal computer, a logistics management server, or a network device) to execute all or part of the steps of the method according to the embodiments of the present application. And the aforementioned storage medium includes: a U-disk, a removable hard disk, a Read-Only Memory (ROM), a Random Access Memory (RAM), a magnetic disk or an optical disk, and other various media capable of storing program codes.
The above embodiments are only used for illustrating the technical solutions of the present application, and not for limiting the same; although the present application has been described in detail with reference to the foregoing embodiments, it should be understood by those of ordinary skill in the art that: the technical solutions described in the foregoing embodiments may still be modified, or some technical features may be equivalently replaced; and such modifications or substitutions do not depart from the spirit and scope of the corresponding technical solutions in the embodiments of the present application.