CN112033401B - Intelligent tunneling robot positioning and orientation method based on strapdown inertial navigation and oil cylinder - Google Patents

Intelligent tunneling robot positioning and orientation method based on strapdown inertial navigation and oil cylinder Download PDF

Info

Publication number
CN112033401B
CN112033401B CN202010950448.5A CN202010950448A CN112033401B CN 112033401 B CN112033401 B CN 112033401B CN 202010950448 A CN202010950448 A CN 202010950448A CN 112033401 B CN112033401 B CN 112033401B
Authority
CN
China
Prior art keywords
robot
inertial navigation
strapdown inertial
intelligent tunneling
error
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.)
Active
Application number
CN202010950448.5A
Other languages
Chinese (zh)
Other versions
CN112033401A (en
Inventor
马宏伟
华洪涛
贺媛
毛清华
李磊
张羽飞
石金龙
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Shenzhen Saiao Aviation Technology Co ltd
Xian University of Science and Technology
Original Assignee
Shenzhen Saiao Aviation Technology Co ltd
Xian University of Science and Technology
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Shenzhen Saiao Aviation Technology Co ltd, Xian University of Science and Technology filed Critical Shenzhen Saiao Aviation Technology Co ltd
Priority to CN202010950448.5A priority Critical patent/CN112033401B/en
Publication of CN112033401A publication Critical patent/CN112033401A/en
Application granted granted Critical
Publication of CN112033401B publication Critical patent/CN112033401B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

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
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T90/00—Enabling technologies or technologies with a potential or indirect contribution to GHG emissions mitigation

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
  • Navigation (AREA)

Abstract

The invention relates to the technical field of coal mining, in particular to an intelligent tunneling robot positioning and orientation method based on strapdown inertial navigation and an oil cylinder. The invention utilizes the displacement data of the advancing of the intelligent tunneling robot by the oil cylinder to compensate the accumulated error of the strapdown inertial navigation, and overcomes the defect that the positioning and orientation error of the intelligent tunneling robot diverges along with time caused by the solution of the pure inertial navigation; the method can adapt to complex environments in the coal mine and realize autonomous measurement of the pose data of the intelligent tunneling robot.

Description

Intelligent tunneling robot positioning and orientation method based on strapdown inertial navigation and oil cylinder
Technical Field
The invention relates to the technical field of coal mining, in particular to an intelligent tunneling robot positioning and orientation method based on strapdown inertial navigation and an oil cylinder.
Background
Coal is an important component of energy in China, and remains its main position for a long time in the future. Along with the continuous improvement of technological innovation ability, the intelligent level of coal mining is greatly improved, and the mining efficiency and the safety are ensured. In the current coal mining process, the accurate positioning and orientation of the heading machine is an important intelligent research direction of the fully-mechanized coal mining face. The tunneling direction of the tunneling machine at the present stage is mostly guided by laser, and the method cannot measure the attitude information of the tunneling machine, so that the attitude information cannot be provided for automatic correction of the tunneling direction of the tunneling machine, and the method cannot adapt to the present requirements in the process of intelligent development of coal mines.
Because the underground environment is too complex, some pose measuring methods which can be used on the ground are not suitable for underground, and inertial navigation is a preferred mode for positioning and orienting underground equipment of a coal mine by virtue of the characteristic that the inertial navigation does not depend on external information and can work in various environments on the ground, underground and in the air in all weather. However, the measurement error of the pure inertial navigation measurement system increases with time.
Disclosure of Invention
Aiming at the problems, the invention provides an intelligent tunneling robot positioning and orientation method based on strapdown inertial navigation and an oil cylinder, which uses the oil cylinder data to calibrate the strapdown inertial belt data so as to realize the accurate positioning and orientation of the intelligent tunneling robot.
In order to achieve the above purpose, the technical scheme adopted by the invention is as follows:
the intelligent tunneling robot positioning and orientation method based on strapdown inertial navigation and an oil cylinder realizes correction of pose data of the intelligent tunneling robot calculated by strapdown inertial navigation based on displacement data of the forward movement of the intelligent tunneling robot by the oil cylinder, thereby realizing high-precision positioning and orientation of the intelligent tunneling robot, wherein the intelligent tunneling robot consists of a robot I and a robot II, the robot I and the robot II are connected through the oil cylinder, and when the intelligent tunneling robot advances, the robot I is pushed to a preset position through the oil cylinder at first, and then the robot II is pulled to the preset position through the oil cylinder, and the method specifically comprises the following steps:
s1, installing strapdown inertial navigation on a robot I, and establishing a carrier coordinate system and a navigation coordinate system;
s101, installing strapdown inertial navigation on a robot I, taking the gravity center of an intelligent tunneling robot as a coordinate origin, taking the advancing direction of the intelligent tunneling robot as a Y-axis positive direction, taking the right side of the intelligent tunneling robot vertical to the Y-axis direction as an X-axis positive direction, taking the upward direction of the intelligent tunneling robot vertical to the Z-axis positive direction, and establishing a carrier coordinate system OXYZ;
s102, taking an east-north-sky coordinate system of a position where strapdown inertial navigation is installed as a navigation coordinate system O 1 X 1 Y 1 Z 1 ；
S103, under a carrier coordinate system, the intelligent tunneling robot rotates around the Z axis to form a heading angle, which is recorded as
Figure BDA0002675718080000021
The angle of rotation around the X axis is called a pitch angle, denoted as θ, and the angle of rotation around the Y axis is called a roll angle, denoted as γ, and the attitude matrix of the intelligent tunneling robot from the navigation coordinate system to the carrier coordinate system is as follows:
Figure BDA0002675718080000022
s2, forming a dead reckoning system by using the strapdown inertial navigation system and the oil cylinder, and analyzing errors contained in the dead reckoning system;
s201, constructing a strapdown inertial navigation error model:
Figure BDA0002675718080000023
Figure BDA0002675718080000024
Figure BDA0002675718080000025
Figure BDA0002675718080000031
Figure BDA0002675718080000032
Figure BDA0002675718080000033
Figure BDA0002675718080000034
Figure BDA0002675718080000035
in the formula ,φE 、φ N 、φ U Respectively calculating misalignment angle error omega between navigation coordinate system and ideal navigation coordinate system E 、ω N 、ω U Measured values of gyroscopes, v E 、v N 、v U The speeds of the intelligent tunneling robots in the navigation coordinate system are respectively,
Figure BDA0002675718080000036
respectively the speed errors of the intelligent tunneling robot in a navigation coordinate system, R Nh 、R Mh Respectively a meridian radius and a mortise unitary circle radius, f E 、f N 、f U Accelerometer measurements, L is the local latitude δL, δλ, δh is the latitude, longitude and altitude errors, respectively, ε E 、ε N 、ε U Zero offset error of gyroscope, +.>
Figure BDA0002675718080000037
Zero offset errors, g, of accelerometers, respectively e Is the equatorial gravity, beta 1 、β 2 0.005302, 3.08X10 respectively -6 、8.08×10 -9 ；
S202, constructing a dead reckoning position error model:
Figure BDA0002675718080000038
Figure BDA0002675718080000039
Figure BDA00026757180800000310
in the formula ,LD 、λ D 、h D Latitude, longitude and altitude of the position of the intelligent tunneling robot during dead reckoning respectively DE 、V DN 、V DU The east speed, the north speed and the sky speed of the intelligent tunneling robot during dead reckoning are respectively,
Figure BDA0002675718080000041
α θ for the installation deviation angle between strapdown inertial navigation and intelligent tunneling robot, delta K D The displacement coefficient error of the oil cylinder;
s203, constructing a lever arm error model:
Figure BDA0002675718080000042
wherein δl is a position vector of the oil cylinder relative to strapdown inertial navigation;
s3, realizing fusion of strapdown inertial navigation and data of the oil cylinder through a standard Kalman filtering algorithm;
s301, comprehensively considering a strapdown inertial navigation error, a dead reckoning error and a lever arm error, and establishing a system state equation of the strapdown inertial navigation and the oil cylinder;
the state variables of the system are as follows:
Figure BDA0002675718080000043
in the formula ,φT To calculate the misalignment angle error between the navigation coordinate system and the ideal navigation coordinate system, (δv) T Speed error of intelligent tunneling robot (δp) T For the position error of the intelligent tunneling robot, (δp) D ) T is the position error during dead reckoning, (δp) GL ) T Is the lever arm error (ε) b ) T Is the zero offset error of the gyroscope,
Figure BDA0002675718080000044
zero offset error for accelerometer,>
Figure BDA0002675718080000045
l is a position vector of the strapdown inertial navigation and the oil cylinder;
the state equation of the combined measurement system is:
Figure BDA0002675718080000046
wherein ,
Figure BDA0002675718080000051
Figure BDA0002675718080000052
Figure BDA0002675718080000053
and />
Figure BDA0002675718080000054
White noise is measured for the angular velocity of the gyroscope and white noise is measured for the specific force of the accelerometer;
according to the error model in S201, S202, S203, the parameters in the state transition matrix F are as follows:
Figure BDA0002675718080000055
Figure BDA0002675718080000056
Figure BDA0002675718080000057
Figure BDA0002675718080000058
Figure BDA0002675718080000059
Figure BDA00026757180800000510
Figure BDA0002675718080000061
s302, establishing a system measurement equation of strapdown inertial navigation and an oil cylinder
Constructing an observation vector from the difference between the strapdown inertial navigation solution position and the dead reckoned position, i.e
Z＝δp-δp D
The measurement equation of the system is
Z＝HX+V
Wherein H= [0 ] 3×6 I 3×3 -I 3×3 0 3×15 ]V is measurement noise;
s303, establishing a strapdown inertial navigation and oil cylinder standard Kalman filtering equation to obtain an optimal estimated value of a system state, wherein the iterative process can be divided into the following 5 steps
1) One-step prediction of the state at the current moment is carried out through a state equation and the state at the last moment of the system
X(k/k-1)＝F·X(k-1)
Wherein X (k-1) is the state quantity of the system at the moment of k-1;
2) Solving a mean square error matrix of the error of the prediction state at the current moment
P(k/k-1)＝FP(k-1)F T +Q
P (k-1) is a mean square error matrix of a system state error at the moment k-1, and Q is a system noise matrix;
3) Solving for Kalman gain K
K k ＝P(k/k-1)H T (HP(k/k-1)H T +R) -1
R is the measurement noise matrix
4) Updating the optimal estimated value of the current moment state by combining with the Kalman gain K
X(k)＝X(k/k-1)+K k (Z k -HX(k/k-1))
5) Updating the mean square error matrix of the optimal estimated value error of the current moment state
P(k)＝(I-K k H)P(k/k-1)
S4, obtaining a pose curve of the intelligent tunneling robot according to a fusion result of the forward displacement data and the strapdown inertial navigation data of the intelligent tunneling robot by pushing the oil cylinder, and realizing accurate positioning and orientation of a tunneling working face.
Further, when the intelligent tunneling robot linearly advances, the pushing amounts of the oil cylinders at the left side and the right side of the intelligent tunneling robot are equal, and at the moment, the displacement amount of the intelligent tunneling robot is equal to the pushing amount of the oil cylinders; when the intelligent tunneling robot rectifies, the pushing amounts of the oil cylinders at the left side and the right side are unequal, and because the strapdown inertial navigation is fixedly connected with the robot I, the displacement of the robot I is equivalent to the displacement of the strapdown inertial navigation, and the displacement of the strapdown inertial navigation can be approximately equivalent to the projection displacement of the intelligent tunneling robot behind the shield body of the robot I, at the moment, the displacement data of the displacement of the intelligent tunneling robot are as follows:
Figure BDA0002675718080000071
wherein a isThe distance between the strapdown inertial navigation projection and the left surface of the I shield body of the robot, b is the distance between the strapdown inertial navigation projection and the right surface of the I shield body of the robot, L l For the pushing quantity of the left cylinder of the intelligent tunneling robot, L r The pushing amount of the right oil cylinder of the intelligent tunneling robot is calculated.
Further, the attitude data of the intelligent tunneling robot obtained by strapdown inertial navigation calculation and the forward displacement data of the intelligent tunneling robot pushed by the oil cylinder are combined to form a dead reckoning system, and an error model of the dead reckoning system is built.
Further, a lever arm error model is constructed by considering lever arm errors generated by different oil cylinder positions and strapdown inertial navigation installation positions.
Further, the error of the strapdown inertial navigation system, the error of the dead reckoning system and the lever arm error are comprehensively considered, and fusion of strapdown inertial navigation data and oil cylinder data is realized by utilizing standard Kalman filtering, so that high-precision positioning and orientation data of the intelligent tunneling robot are obtained.
The invention has the following beneficial effects:
(1) The displacement data of the intelligent tunneling robot advancing is pushed by the oil cylinder to compensate the accumulated error of the strapdown inertial navigation, so that the defect that the positioning and orientation errors of the intelligent tunneling robot diverge along with time due to the calculation of the pure inertial navigation is overcome;
(2) The method can adapt to complex environments in the coal mine and realize autonomous measurement of the pose data of the intelligent tunneling robot.
Drawings
Fig. 1 is a schematic diagram of the present invention.
FIG. 2 is a schematic installation view of the present invention;
in the figure: 1-an oil cylinder; 2-an oil cylinder base; 3-strapdown inertial navigation; 4-robot I; 5-robot II; 6-inertial navigation projection position.
FIG. 3 is a schematic illustration of the correction of the present invention.
Fig. 4 is a coordinate system diagram of the present invention.
Fig. 5 is a flow chart of the present invention.
Detailed Description
The present invention will be described in detail with reference to specific examples. The following examples will assist those skilled in the art in further understanding the present invention, but are not intended to limit the invention in any way. It should be noted that variations and modifications could be made by those skilled in the art without departing from the inventive concept. These are all within the scope of the present invention.
The embodiment of the invention provides an intelligent tunneling robot positioning and orientation method based on strapdown inertial navigation and an oil cylinder, wherein the intelligent tunneling robot consists of a robot I and a robot II, the robot I and the robot II are connected through the oil cylinder, when the intelligent tunneling robot advances, the robot I is pushed to a preset position through the oil cylinder, and then the robot II is pulled to the preset position through the oil cylinder, and the positioning and orientation method comprises the following steps:
s1, installing strapdown inertial navigation on a robot I, and establishing a carrier coordinate system and a navigation coordinate system;
s101, taking the gravity center of an intelligent tunneling robot as a coordinate origin, taking the advancing direction of the intelligent tunneling robot as a Y-axis positive direction, taking the direction vertical to the Y-axis on the right side of the intelligent tunneling robot as an X-axis positive direction, taking the upward direction vertical to the intelligent tunneling robot as a Z-axis positive direction, and establishing a carrier coordinate system OXYZ;
s102, taking an east-north-sky coordinate system of a position where strapdown inertial navigation is installed as a navigation coordinate system O 1 X 1 Y 1 Z 1 ；
S103, under a carrier coordinate system, the intelligent tunneling robot rotates around the Z axis to form a heading angle, which is recorded as
Figure BDA0002675718080000081
The angle of rotation around the X axis is called a pitch angle, denoted as θ, and the angle of rotation around the Y axis is called a roll angle, denoted as γ, and the attitude matrix of the intelligent tunneling robot from the navigation coordinate system to the carrier coordinate system is as follows:
Figure BDA0002675718080000091
s2, when the intelligent tunneling robot linearly advances, the pushing amounts of the oil cylinders at the left side and the right side of the intelligent tunneling robot are equal, and at the moment, the displacement amount of the intelligent tunneling robot is equal to the pushing amount of the oil cylinders; when the intelligent tunneling robot rectifies, the displacement of the oil cylinders at the left side and the right side is unequal, because the strapdown inertial navigation is fixedly connected with the robot I, the displacement of the robot I is equivalent to the displacement of the strapdown inertial navigation, the displacement of the strapdown inertial navigation can be approximately equivalent to the projection displacement of the strapdown inertial navigation behind the shield body of the robot I (see figure 3), and at the moment, the mathematical model of the displacement of the intelligent tunneling robot is as follows
Figure BDA0002675718080000092
Wherein a is the distance between the strapdown inertial navigation projection and the left surface of the I shield body of the robot, b is the distance between the strapdown inertial navigation projection and the right surface of the I shield body of the robot, and L l For the pushing quantity of the left cylinder of the intelligent tunneling robot, L r The pushing amount of the right oil cylinder of the intelligent tunneling robot is calculated;
s3, forming a dead reckoning system by using the strapdown inertial navigation system and the oil cylinder, and analyzing errors contained in the dead reckoning system, wherein the specific results are as follows:
s301, strapdown inertial navigation error model:
Figure BDA0002675718080000093
Figure BDA0002675718080000094
Figure BDA0002675718080000095
Figure BDA0002675718080000096
Figure BDA0002675718080000101
Figure BDA0002675718080000102
Figure BDA0002675718080000103
Figure BDA0002675718080000104
Figure BDA0002675718080000105
in the formula ,φE 、φ N 、φ U Respectively calculating misalignment angle error omega between navigation coordinate system and ideal navigation coordinate system E 、ω N 、ω U Measured values of gyroscopes, v E 、v N 、v U The speeds of the intelligent tunneling robots in the navigation coordinate system are respectively,
Figure BDA0002675718080000106
respectively the speed errors of the intelligent tunneling robot in a navigation coordinate system, R Nh 、R Mh Respectively a meridian radius and a mortise unitary circle radius, f E 、f N 、f U Accelerometer measurements, L is the local latitude, δL, δλ, δh is the latitude, longitude and altitude errors, ε, respectively E 、ε N 、ε U Zero offset error of gyroscope, +.>
Figure BDA0002675718080000107
Zero offset errors, g, of accelerometers, respectively e Is the equatorial gravity, beta 1 、β 2 0.005302, 3.08X10 respectively -6 、8.08×10 -9 ；
S302, a dead reckoning position error model is as follows:
Figure BDA0002675718080000108
Figure BDA0002675718080000109
Figure BDA00026757180800001010
in the formula ,LD 、λ D 、h D Latitude, longitude and altitude of the position of the intelligent tunneling robot during dead reckoning respectively DE 、V DN 、V DU The east speed, the north speed and the sky speed of the intelligent tunneling robot during dead reckoning are respectively,
Figure BDA00026757180800001011
α θ for the installation deviation angle between strapdown inertial navigation and intelligent tunneling robot, delta K D The displacement coefficient error of the oil cylinder;
s303, a lever arm error model is as follows:
Figure BDA0002675718080000111
wherein δl is a position vector of the oil cylinder relative to strapdown inertial navigation.
S4, fusing the strapdown inertial navigation data and the data of the oil cylinder through a standard Kalman filtering algorithm, wherein the method comprises the following steps of:
s401, comprehensively considering the strapdown inertial navigation error, dead reckoning error and lever arm error, and establishing a system state equation of the strapdown inertial navigation and the oil cylinder;
the state variables of the system are as follows:
Figure BDA0002675718080000116
in the formula ,φT To calculate the misalignment angle error between the navigation coordinate system and the ideal navigation coordinate system, (δv) T Speed error of intelligent tunneling robot (δp) T For the position error of the intelligent tunneling robot, (δp) D ) T For dead reckoning position errors, (δp) GL ) T Is the lever arm error (ε) b ) T Is the zero offset error of the gyroscope,
Figure BDA0002675718080000112
zero offset error for accelerometer,>
Figure BDA0002675718080000113
and l is a position vector of the strapdown inertial navigation and the oil cylinder. The state equation of the combined measurement system is:
Figure BDA0002675718080000114
wherein ,
Figure BDA0002675718080000115
Figure BDA0002675718080000121
Figure BDA0002675718080000122
and />
Figure BDA0002675718080000123
White noise is measured for the angular velocity of the gyroscope and white noise is measured for the specific force of the accelerometer;
according to the error model in S201, S202, S203, the parameters in the state transition matrix F are as follows:
Figure BDA0002675718080000124
Figure BDA0002675718080000125
Figure BDA0002675718080000126
Figure BDA0002675718080000127
Figure BDA0002675718080000128
Figure BDA0002675718080000129
Figure BDA00026757180800001210
s402, establishing a system measurement equation of strapdown inertial navigation and an oil cylinder
Constructing an observation vector from the difference between the strapdown inertial navigation solution position and the dead reckoned position, i.e
Z＝δp-δp D
The measurement equation of the system is
Z＝HX+V
Wherein H= [ O ] 3×6 I 3×3 -I 3×3 O 3×15 ]V is measurement noise;
s403, establishing a strapdown inertial navigation and oil cylinder standard Kalman filtering equation to obtain an optimal estimated value of a system state, wherein the iterative process can be divided into the following 5 steps
1) One-step prediction of the state at the current moment is carried out through a state equation and the state at the last moment of the system
X(k/k-1)＝F·X(k-1)
Wherein X (k-1) is the state quantity of the system at time k-1.
2) Solving a mean square error matrix of the error of the prediction state at the current moment
P(k/k-1)＝FP(k-1)F T +Q
P (k-1) is a mean square error matrix of the system state error at the moment k-1, and Q is a system noise matrix.
3) Solving for Kalman gain K
K k ＝P(k/k-1)H T (HP(k/k-1)H T +R) -1
Wherein R is a measurement noise matrix
4) Updating the optimal estimated value of the current moment state by combining with the Kalman gain K
X(k)＝X(k/k-1)+K k (Z k -HX(k/k-1))
5) Updating the mean square error matrix of the optimal estimated value error of the current moment state
P(k)＝(I-K k H)P(k/k-1)
S6, obtaining a pose curve of the intelligent tunneling robot according to a fusion result of the forward displacement data and the strapdown inertial navigation data of the intelligent tunneling robot by pushing the oil cylinder, and realizing accurate positioning and orientation of a tunneling working face.
The foregoing describes specific embodiments of the present invention. It is to be understood that the invention is not limited to the particular embodiments described above, and that various changes or modifications may be made by those skilled in the art within the scope of the appended claims without affecting the spirit of the invention. The embodiments of the present application and features in the embodiments may be combined with each other arbitrarily without conflict.

Claims (5)

1. The intelligent tunneling robot positioning and orientation method based on the strapdown inertial navigation and the oil cylinder is characterized in that correction of pose data of the intelligent tunneling robot calculated by strapdown inertial navigation is realized based on displacement data of the advancing of the intelligent tunneling robot by the oil cylinder, so that high-precision positioning and orientation of the intelligent tunneling robot are realized, wherein the intelligent tunneling robot consists of a robot I and a robot II, the robot I and the robot II are connected through the oil cylinder, and when the intelligent tunneling robot advances, the robot I is pushed to a preset position through the oil cylinder at first, and then the robot II is pulled to the preset position through the oil cylinder;
the intelligent tunneling robot positioning and orientation method based on strapdown inertial navigation and an oil cylinder comprises the following steps:
s1, installing strapdown inertial navigation on a robot I, and establishing a carrier coordinate system and a navigation coordinate system;
s101, installing strapdown inertial navigation on a robot I, taking the gravity center of an intelligent tunneling robot as a coordinate origin, taking the advancing direction of the intelligent tunneling robot as a Y-axis positive direction, taking the right side of the intelligent tunneling robot vertical to the Y-axis direction as an X-axis positive direction, taking the upward direction of the intelligent tunneling robot vertical to the Z-axis positive direction, and establishing a carrier coordinate system OXYZ;
s102, taking an east-north-sky coordinate system of a position where strapdown inertial navigation is installed as a navigation coordinate system O 1 X 1 Y 1 Z 1 ；
S103, under a carrier coordinate system, the intelligent tunneling robot rotates around the Z axis to form a heading angle, which is recorded as
Figure FDA0004223645540000011
The angle of rotation around the X axis is called a pitch angle, denoted as theta, the angle of rotation around the Y axis is called a roll angle, denoted as gamma, and the attitude transformation matrix of the intelligent tunneling robot from the navigation coordinate system to the carrier coordinate system is>
Figure FDA0004223645540000012
The following are provided:
Figure FDA0004223645540000013
s2, forming a dead reckoning system by using the strapdown inertial navigation system and the oil cylinder, and analyzing errors contained in the dead reckoning system;
s201, constructing a strapdown inertial navigation error model:
Figure FDA0004223645540000021
Figure FDA0004223645540000022
Figure FDA0004223645540000023
Figure FDA0004223645540000024
Figure FDA0004223645540000025
Figure FDA0004223645540000026
Figure FDA0004223645540000027
Figure FDA0004223645540000028
Figure FDA0004223645540000029
in the formula ,φE 、φ N 、φ U Respectively calculating misalignment angle error omega between navigation coordinate system and ideal navigation coordinate system E 、ω N 、ω U Measured values of gyroscopes, v E 、v N 、v U Respectively the speeds of the intelligent tunneling robot in a navigation coordinate system, δv E 、δv N 、δv U Respectively the speed errors of the intelligent tunneling robot in a navigation coordinate system, R Nh 、R Mh Respectively a meridian radius and a mortise unitary circle radius, f E 、f N 、f U Accelerometer measurements, L is the local latitude, δL, δλ, δh is the latitude, longitude and altitude errors, ε, respectively E 、ε N 、ε U The zero offset errors of the gyroscopes are respectively determined,
Figure FDA00042236455400000210
zero offset errors, g, of accelerometers, respectively e Is the equatorial gravity, beta 1 、β 2 0.005302, 3.08X10 respectively -6 、8.08×10 -9 ；
S202, constructing a dead reckoning position error model:
Figure FDA0004223645540000031
Figure FDA0004223645540000032
Figure FDA0004223645540000033
in the formula ,LD 、λ D 、h D The latitude, longitude and altitude of the position of the intelligent tunneling robot during dead reckoning are respectively,
Figure FDA0004223645540000034
α θ for the installation deviation angle between strapdown inertial navigation and intelligent tunneling robot, delta K D The displacement coefficient error of the oil cylinder;
s203, constructing a lever arm error model:
Figure FDA0004223645540000035
wherein δl is a position vector of the oil cylinder relative to strapdown inertial navigation;
s3, realizing fusion of strapdown inertial navigation and data of the oil cylinder through a standard Kalman filtering algorithm;
s301, comprehensively considering a strapdown inertial navigation error, a dead reckoning error and a lever arm error, and establishing a system state equation of the strapdown inertial navigation and the oil cylinder;
wherein, the state variable X of the system is:
Figure FDA0004223645540000036
in the formula ,φT To calculate the misalignment angle error between the navigation coordinate system and the ideal navigation coordinate system, (δv) T Speed error of intelligent tunneling robot (δp) T For the position error of the intelligent tunneling robot, (δp) D ) T For dead reckoning position errors, (δp) GL ) T Is the lever arm error (ε) b ) T Is the zero offset error of the gyroscope,
Figure FDA0004223645540000041
zero offset error for accelerometer,>
Figure FDA0004223645540000042
l is a position vector of the strapdown inertial navigation and the oil cylinder;
the state equation of the combined measurement system is:
Figure FDA0004223645540000043
wherein ,
Figure FDA0004223645540000044
Figure FDA0004223645540000045
Figure FDA0004223645540000046
and />
Figure FDA0004223645540000047
White noise is measured for the angular velocity of the gyroscope and white noise is measured for the specific force of the accelerometer;
according to the error model in S201, S202, S203, the parameters in the state transition matrix F are as follows:
Figure FDA0004223645540000048
Figure FDA0004223645540000049
Figure FDA00042236455400000410
Figure FDA0004223645540000051
Figure FDA0004223645540000052
Figure FDA0004223645540000053
Figure FDA0004223645540000054
Figure FDA0004223645540000055
s302, establishing a system measurement equation of strapdown inertial navigation and an oil cylinder
Constructing the observation vector Z as the difference between the strapdown inertial navigation solution position and the dead reckoned position, i.e
Z＝δp-δp D
The measurement equation of the system is z=hx+v
Wherein H= [0 ] 3×6 I 3×3 -I 3×3 0 3×15 ]V is measurement noise;
s303, establishing a strapdown inertial navigation and oil cylinder standard Kalman filtering equation to obtain an optimal estimated value of a system state, wherein the iterative process can be divided into the following 5 steps:
1) One-step prediction of the state at the current moment is carried out through a state equation and the state at the last moment of the system
X(k/k-1)＝F·X(k-1)
Wherein X (k/k-1) is a one-step prediction state quantity, and X (k-1) is a state quantity of the system at the moment of k-1;
2) Solving a mean square error matrix P (k/k-1) of the error of the prediction state at the current moment
P(k/k-1)＝FP(k-1)F T +Q
P (k-1) is a mean square error matrix of a system state error at the moment k-1, and Q is a system noise matrix;
3) Solving for Kalman increasesBenefit K k
K k ＝P(k/k-1)H T (HP(k/k-1)H T +R) -1
Wherein R is a measurement noise matrix;
4) Combining Kalman gain K k Updating the optimal estimation value of the current time state
X(k)＝X(k/k-1)+K k (Z k -HX(k/k-1))
5) Updating the mean square error matrix of the optimal estimated value error of the current moment state
P(k)＝(I-K k H)P(k/k-1)
S4, obtaining a pose curve of the intelligent tunneling robot according to a fusion result of the forward displacement data and the strapdown inertial navigation data of the intelligent tunneling robot by pushing the oil cylinder, and realizing accurate positioning and orientation of a tunneling working face.
2. The intelligent tunneling robot positioning and orientation method based on strapdown inertial navigation and oil cylinders, as set forth in claim 1, is characterized in that: when the intelligent tunneling robot linearly advances, the pushing amounts of the oil cylinders at the left side and the right side of the intelligent tunneling robot are equal, and at the moment, the displacement amount of the intelligent tunneling robot is equal to the pushing amount of the oil cylinders; when the intelligent tunneling robot rectifies, the pushing amounts of the oil cylinders at the left side and the right side are unequal, and because the strapdown inertial navigation is fixedly connected with the robot I, the displacement of the robot I is equivalent to the displacement of the strapdown inertial navigation, and the displacement of the strapdown inertial navigation can be approximately equivalent to the projection displacement of the intelligent tunneling robot behind the shield body of the robot I, at the moment, the displacement data L of the displacement of the intelligent tunneling robot are as follows:
Figure FDA0004223645540000061
wherein a is the distance between the strapdown inertial navigation projection and the left surface of the I shield body of the robot, b is the distance between the strapdown inertial navigation projection and the right surface of the I shield body of the robot, and L l For the pushing quantity of the left cylinder of the intelligent tunneling robot, L r The pushing amount of the right oil cylinder of the intelligent tunneling robot is calculated.
3. The intelligent tunneling robot positioning and orientation method based on strapdown inertial navigation and oil cylinders, as set forth in claim 1, is characterized in that: and combining the attitude data of the intelligent tunneling robot obtained by strapdown inertial navigation calculation with the forward displacement data of the intelligent tunneling robot pushed by the oil cylinder to form a dead reckoning system, and constructing an error model of the dead reckoning system.
4. The intelligent tunneling robot positioning and orientation method based on strapdown inertial navigation and oil cylinders, as set forth in claim 1, is characterized in that: and taking lever arm errors generated by different positions of the oil cylinder and the strapdown inertial navigation installation position into consideration, and constructing a lever arm error model.
5. The intelligent tunneling robot positioning and orientation method based on strapdown inertial navigation and oil cylinders, as set forth in claim 1, is characterized in that: and comprehensively considering the error of the strapdown inertial navigation system, the error of the dead reckoning system and the lever arm error, and realizing fusion of strapdown inertial navigation data and oil cylinder data by utilizing standard Kalman filtering to obtain high-precision positioning and orientation data of the intelligent tunneling robot.
CN202010950448.5A 2020-09-10 2020-09-10 Intelligent tunneling robot positioning and orientation method based on strapdown inertial navigation and oil cylinder Active CN112033401B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010950448.5A CN112033401B (en) 2020-09-10 2020-09-10 Intelligent tunneling robot positioning and orientation method based on strapdown inertial navigation and oil cylinder

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010950448.5A CN112033401B (en) 2020-09-10 2020-09-10 Intelligent tunneling robot positioning and orientation method based on strapdown inertial navigation and oil cylinder

Publications (2)

Publication Number Publication Date
CN112033401A CN112033401A (en) 2020-12-04
CN112033401B true CN112033401B (en) 2023-06-20

Family

ID=73588849

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010950448.5A Active CN112033401B (en) 2020-09-10 2020-09-10 Intelligent tunneling robot positioning and orientation method based on strapdown inertial navigation and oil cylinder

Country Status (1)

Country Link
CN (1) CN112033401B (en)

Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2007263160A (en) * 2006-03-27 2007-10-11 Kayaba Ind Co Ltd Energy conversion device
CN110095135A (en) * 2019-06-03 2019-08-06 中南大学 A kind of method and device for development machine positioning and directing

Family Cites Families (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US7139651B2 (en) * 2004-03-05 2006-11-21 Modular Mining Systems, Inc. Multi-source positioning system for work machines
JP5328830B2 (en) * 2011-03-24 2013-10-30 株式会社小松製作所 Hydraulic excavator calibration apparatus and hydraulic excavator calibration method
CN105636658B (en) * 2014-05-14 2018-03-23 株式会社小松制作所 The correction system of hydraulic crawler excavator and bearing calibration
US10401176B2 (en) * 2017-06-21 2019-09-03 Caterpillar Inc. System and method for determining machine state using sensor fusion
CN107270901B (en) * 2017-08-17 2020-02-14 中国矿业大学 Coal mining machine inertial positioning precision improving method fusing coal mining process and coal mining machine motion model
CN109540130A (en) * 2018-10-25 2019-03-29 北京航空航天大学 A kind of continuous milling machine inertial navigation positioning and orienting method
CN110162036A (en) * 2019-04-09 2019-08-23 中国矿业大学 A kind of development machine Camera calibration system and method
CN110996048B (en) * 2019-11-20 2021-01-29 中国煤炭科工集团太原研究院有限公司 Remote visualization system and method for coal roadway heading machine

Patent Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2007263160A (en) * 2006-03-27 2007-10-11 Kayaba Ind Co Ltd Energy conversion device
CN110095135A (en) * 2019-06-03 2019-08-06 中南大学 A kind of method and device for development machine positioning and directing

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
基于DGPS定位与双闭环转向控制的农业自动导航系统;黎永键;赵祚喜;高俊文;吴晓鹏;关伟;;农业现代化研究;37(02);全文 *

Also Published As

Publication number Publication date
CN112033401A (en) 2020-12-04

Similar Documents

Publication Publication Date Title
CN102809377B (en) Aircraft inertia/pneumatic model Combinated navigation method
CN103424114B (en) A kind of full combined method of vision guided navigation/inertial navigation
CN110926468B (en) Communication-in-motion antenna multi-platform navigation attitude determination method based on transfer alignment
CN111156994B (en) INS/DR & GNSS loose combination navigation method based on MEMS inertial component
CN110702109B (en) Coal mining machine inertial navigation/wireless sensor network combined positioning method
CN112378399B (en) Coal mine tunnel tunneling robot precise positioning and orientation method based on strapdown inertial navigation and digital total station
CN109540130A (en) A kind of continuous milling machine inertial navigation positioning and orienting method
CN105259902A (en) Inertial navigation method and system of underwater robot
CN111637888B (en) Tunneling machine positioning method and system based on inertial navigation and laser radar single-point distance measurement
CN110207691B (en) Multi-unmanned vehicle collaborative navigation method based on data link ranging
CN110792430A (en) While-drilling inclination measurement method and device based on multi-sensor data fusion
CN112414394A (en) Real-time positioning system and method for underground roadway driving equipment
CN104515527A (en) Anti-rough error integrated navigation method under non-GPS signal environment
CN108931244A (en) Ins error suppressing method and system based on train kinematic constraint
CN112628524B (en) High-precision positioning method for small-diameter pipeline robot based on turning angle
CN115773751A (en) Method for correcting alignment error caused by zero position of equivalent antenna direction adder
CN111207743B (en) Method for realizing centimeter-level accurate positioning based on close coupling of encoder and inertial equipment
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
CN110095135B (en) Method and device for positioning and orienting heading machine
Sun et al. A vehicle-carried INS positioning accuracy improvement method by using lateral constraint in GPS-denied environment
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
CN112033401B (en) Intelligent tunneling robot positioning and orientation method based on strapdown inertial navigation and oil cylinder
CN114061574B (en) Position-invariant constraint and zero-speed correction-based coal mining machine pose-determining and orienting method

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
GR01 Patent grant
GR01 Patent grant