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

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

Info

Publication number
CN112033401A
CN112033401A CN202010950448.5A CN202010950448A CN112033401A CN 112033401 A CN112033401 A CN 112033401A CN 202010950448 A CN202010950448 A CN 202010950448A CN 112033401 A CN112033401 A CN 112033401A
Authority
CN
China
Prior art keywords
inertial navigation
robot
strapdown inertial
oil cylinder
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.)
Granted
Application number
CN202010950448.5A
Other languages
Chinese (zh)
Other versions
CN112033401B (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)
  • Navigation (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (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 accumulated error of the strapdown inertial navigation is compensated by using the displacement data of the intelligent tunneling robot advanced by the oil cylinder, so that the defect that the positioning and orientation error of the intelligent tunneling robot is dispersed along with time due to pure inertial navigation calculation is overcome; the method can adapt to the complex environment in the underground coal mine, and realizes the autonomous measurement of the pose data of the intelligent tunneling robot.

Description

Intelligent tunneling robot positioning and orienting 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 orienting method based on strapdown inertial navigation and an oil cylinder.
Background
Coal, as an important component of our energy, will remain its primary status for a long time in the future. Along with the continuous promotion of science and technology innovation ability, the intelligent level of coal mining improves greatly, and mining efficiency and security have obtained the guarantee. In the current coal mining process, the accurate positioning and orientation of the development machine is an important research direction for the intellectualization of the fully mechanized excavation face. The heading direction of the heading machine at the present stage is mostly guided by laser, and the method cannot measure attitude information of the heading machine, so that the attitude information cannot be provided for automatic deviation correction of the heading direction of the heading machine, and the method cannot adapt to the present requirements in the process of intelligent development of a coal mine.
Because the underground environment is too complex, some pose measurement methods which can be used on the ground are not suitable for underground, and inertial navigation can work in various environments on the ground, underground and in the air all day by relying on the characteristics of not depending on external information, so that the pose measurement method becomes a preferred mode for positioning and orienting underground equipment of a coal mine. However, the measurement error of the pure inertial navigation measurement system is larger and larger with the increase of time.
Disclosure of Invention
Aiming at the problems, the invention provides an intelligent tunneling robot positioning and orienting method based on strapdown inertial navigation and an oil cylinder, which is used for calibrating strapdown inertial belt data by using oil cylinder data to realize accurate positioning and orienting of an intelligent tunneling robot.
In order to achieve the purpose, the invention adopts the technical scheme that:
the intelligent tunneling robot positioning and orienting method based on the strapdown inertial navigation and the oil cylinder is characterized in that the intelligent tunneling robot is pushed by the oil cylinder to correct pose data of the intelligent tunneling robot calculated by the strapdown inertial navigation based on forward displacement data of the intelligent tunneling robot, so that the high-precision positioning and orienting of the intelligent tunneling robot is realized, the intelligent tunneling robot is composed 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 moves forward, a preset position pushed by the robot I is firstly pushed through the oil cylinder, 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 the strapdown inertial navigation system on the 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 the positive direction of a Y axis, taking the direction perpendicular to the Y axis on the right side of the intelligent tunneling robot as the positive direction of an X axis, taking the upward direction perpendicular to the intelligent tunneling robot as the positive direction of a Z axis, and establishing a carrier coordinate system OXYZ;
s102, taking an east-north-sky coordinate system of the position where the strapdown inertial navigation device is arranged as a navigation coordinate system O1X1Y1Z1；
S103, under a carrier coordinate system, the angle of rotation of the intelligent tunneling robot around the Z axis is called a course angle and is recorded as a course angle
Figure BDA0002675718080000021
The angle of rotation around the X axis is called a pitch angle and is recorded as theta, the angle of rotation around the Y axis is called a roll angle and is recorded as gamma, and then the attitude matrix from the navigation coordinate system to the carrier coordinate system of the intelligent tunneling robot is as follows:
Figure BDA0002675718080000022
s2, forming a dead reckoning system by the strapdown inertial navigation 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、φURespectively, for calculating the misalignment angle error, omega, between the navigation coordinate system and the ideal navigation coordinate systemE、ωN、ωURespectively, measured values of a gyroscope, vE、vN、vURespectively the speed of the intelligent tunneling robot in the navigation coordinate system,
Figure BDA0002675718080000036
respectively the speed error R of the intelligent tunneling robot in the navigation coordinate systemNh、RMhAre respectively provided withIs the radius of meridian circle and the radius of unitary and fourth unit circle, fE、fN、fURespectively, accelerometer measurements, L is the local latitude, L, λ, h are latitude, longitude and altitude errors, respectively,E、N、Urespectively, are the zero-offset errors of the gyroscope,
Figure BDA0002675718080000037
respectively, the zero offset error, g, of the accelerometereIs equatorial gravity, beta1、β20.005302, 3.08X 10 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、hDRespectively is latitude, longitude and altitude V of the position of the intelligent tunneling robot during dead reckoningDE、VDN、VDURespectively an east speed, a north speed and a sky speed of the intelligent tunneling robot during dead reckoning,
Figure BDA0002675718080000041
αθfor the installation deviation angle, K, between strapdown inertial navigation and intelligent tunnelling robotDThe oil cylinder shift coefficient error is obtained;
s203, constructing a lever arm error model:
Figure BDA0002675718080000042
in the formula, l is a position vector of the oil cylinder relative to the strapdown inertial navigation;
s3, fusing data of the strapdown inertial navigation and the oil cylinder through a standard Kalman filtering algorithm;
s301, comprehensively considering strapdown inertial navigation errors, dead reckoning errors and lever arm errors, and establishing a system state equation of the strapdown inertial navigation and the oil cylinder;
wherein, the state variables of the system are as follows:
Figure BDA0002675718080000043
in the formula ,φTTo calculate the misalignment angle error between the navigation coordinate system and the ideal navigation coordinate system, (v)TFor the speed error of the intelligent tunneling robot, (p)TPosition error for intelligent tunneling robot, (p)D) T is a position error in dead reckoning, (p)GL)TFor lever arm error: (b)TThe error is the zero offset error of the gyroscope,
Figure BDA0002675718080000044
for the zero offset error of the accelerometer,
Figure BDA0002675718080000045
l is the position vector of the strapdown inertial navigation and the oil cylinder;
the state equation of the combined measurement system is as follows:
Figure BDA0002675718080000046
wherein ,
Figure BDA0002675718080000051
Figure BDA0002675718080000052
Figure BDA0002675718080000053
and
Figure BDA0002675718080000054
white noise for measuring the angular velocity of the gyroscope and white noise for measuring the specific force of the accelerometer respectively;
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 oil cylinder
The difference between the position resolved by strapdown inertial navigation and the position of dead reckoning is used to construct an observation vector, i.e.
Z＝p-pD
The measurement equation of the system is
Z＝HX+V
Wherein H is [0 ]3×6 I3×3 -I3×3 03×15]V is measurement noise;
s303, establishing a strapdown inertial navigation and oil cylinder standard Kalman filtering equation to obtain an optimal estimation value of the system state, wherein the iteration process can be divided into the following 5 steps
1) One-step prediction is carried out on the current time state through the state equation and the state of the last time of the system
X(k/k-1)＝F·X(k-1)
Wherein X (k-1) is the state quantity of the system at the k-1 moment;
2) solving the mean square error matrix of the error of the prediction state at the current moment
P(k/k-1)＝FP(k-1)FT+Q
P (k-1) is a mean square error matrix of the system state error at the k-1 moment, and Q is a system noise matrix;
3) solving Kalman gain K
Kk＝P(k/k-1)HT(HP(k/k-1)HT+R)-1
R is a measurement noise matrix
4) Updating the optimal estimation value of the current time state by combining Kalman gain K
X(k)＝X(k/k-1)+Kk(Zk-HX(k/k-1))
5) Updating the mean square error matrix of the optimal estimation value error of the current time state
P(k)＝(I-KkH)P(k/k-1)
And S4, obtaining a pose curve of the intelligent tunneling robot according to the fusion result of the displacement data of the intelligent tunneling robot pushed by the oil cylinder and the strapdown inertial navigation data, and realizing accurate positioning and orientation of the tunneling working face.
Further, when the intelligent tunneling robot moves forwards linearly, the oil cylinder displacement amount of the left side and the right side of the intelligent tunneling robot is equal, and at the moment, the displacement amount of the intelligent tunneling robot is equal to the oil cylinder displacement amount; when the intelligent tunneling robot corrects the deviation, the oil cylinder pushing amount on the left side and the right side is unequal, and the strapdown inertial navigation is fixedly connected with the robot I, so that 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 I of the robot, and the displacement data of the intelligent tunneling robot is as follows:
Figure BDA0002675718080000071
wherein a is the distance between the strapdown inertial navigation projection and the left surface of the robot shield I, b is the distance between the strapdown inertial navigation projection and the right surface of the robot shield I, and LlFor the displacement, L, of the left side oil cylinder of the intelligent tunneling robotrThe intelligent tunneling robot is the displacement of the oil cylinder on the right side.
Furthermore, combining the attitude data of the intelligent tunneling robot obtained by the strapdown inertial navigation and the displacement data of the intelligent tunneling robot pushed by the oil cylinder to advance to form a dead reckoning system, and constructing an error model of the dead reckoning system.
Further, a lever arm error generated by the fact that the oil cylinder position is different from the strapdown inertial navigation installation position is considered, and a lever arm error model is built.
Further, the error of the strapdown inertial navigation system, the error of the dead reckoning system and the lever arm error are comprehensively considered, the fusion of strapdown inertial navigation data and oil cylinder data is achieved through standard Kalman filtering, and high-precision positioning and orientation data of the intelligent tunneling robot are obtained.
The invention has the following beneficial effects:
(1) the accumulated error of the strapdown inertial navigation is compensated by using the displacement data of the intelligent tunneling robot advanced by the oil cylinder, so that the defect that the positioning and orientation error of the intelligent tunneling robot is dispersed along with time due to pure inertial navigation calculation is overcome;
(2) the method can adapt to the complex environment in the underground coal mine, and realizes the 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 view of the installation of the present invention;
in the figure: 1-oil cylinder; 2-oil cylinder base; 3-strapdown inertial navigation; 4-robot I; 5-robot II; 6-inertial navigation projection position.
FIG. 3 is a schematic diagram of the rectification 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 invention, but are not intended to limit the invention in any way. It should be noted that variations and modifications can be made by persons skilled in the art without departing from the spirit of the invention. All falling within the scope of the present invention.
The embodiment of the invention provides an intelligent tunneling robot positioning and orienting 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 moves forward, firstly, 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 orienting method comprises the following steps:
s1, installing the strapdown inertial navigation system on the robot I, and establishing a carrier coordinate system and a navigation coordinate system;
s101, establishing a carrier coordinate system OXYZ by taking the gravity center of the intelligent tunneling robot as a coordinate origin, taking the advancing direction of the intelligent tunneling robot as the positive direction of a Y axis, taking the direction of the right side of the intelligent tunneling robot perpendicular to the Y axis as the positive direction of an X axis, and taking the upward direction perpendicular to the intelligent tunneling robot as the positive direction of a Z axis;
s102, taking an east-north-sky coordinate system of the position where the strapdown inertial navigation device is arranged as a navigation coordinate system O1X1Y1Z1；
S103、Under a carrier coordinate system, the angle of rotation around the Z axis of the intelligent tunneling robot is called a course angle and is recorded as a heading angle
Figure BDA0002675718080000081
The angle of rotation around the X axis is called a pitch angle and is recorded as theta, the angle of rotation around the Y axis is called a roll angle and is recorded as gamma, and then the attitude matrix from the navigation coordinate system to the carrier coordinate system of the intelligent tunneling robot is as follows:
Figure BDA0002675718080000091
s2, when the intelligent tunneling robot moves forwards linearly, the oil cylinder shifting amount of the left side and the right side of the intelligent tunneling robot is equal, and at the moment, the intelligent tunneling robot shifting amount is equal to the oil cylinder shifting amount; when the intelligent tunneling robot corrects the deviation, the oil cylinder displacement amount on the left side and the right side is unequal, and the strapdown inertial navigation is fixedly connected with the robot I, so that 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 by using 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 robot shield I, b is the distance between the strapdown inertial navigation projection and the right surface of the robot shield I, and LlFor the displacement, L, of the left side oil cylinder of the intelligent tunneling robotrThe displacement of the oil cylinder on the right side of the intelligent tunneling robot is measured;
s3, forming a dead reckoning system by the strapdown inertial navigation and the oil cylinder, and analyzing errors contained in the dead reckoning system, wherein the specific result is as follows:
s301, a 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、φURespectively, for calculating the misalignment angle error, omega, between the navigation coordinate system and the ideal navigation coordinate systemE、ωN、ωURespectively, measured values of a gyroscope, vE、vN、vURespectively the speed of the intelligent tunneling robot in the navigation coordinate system,
Figure BDA0002675718080000106
respectively the speed error R of the intelligent tunneling robot in the navigation coordinate systemNh、RMhAre respectively half of meridian circleRadius and radius of unit of fourth quarter fE、fN、fURespectively, accelerometer measurements, L is the local latitude, L, λ, h are latitude, longitude and altitude errors, respectively,E、N、Urespectively, are the zero-offset errors of the gyroscope,
Figure BDA0002675718080000107
respectively, the zero offset error, g, of the accelerometereIs equatorial gravity, beta1、β20.005302, 3.08X 10 respectively-6、8.08×10-9；
S302, the dead reckoning position error model is as follows:
Figure BDA0002675718080000108
Figure BDA0002675718080000109
Figure BDA00026757180800001010
in the formula ,LD、λD、hDRespectively is latitude, longitude and altitude V of the position of the intelligent tunneling robot during dead reckoningDE、VDN、VDURespectively an east speed, a north speed and a sky speed of the intelligent tunneling robot during dead reckoning,
Figure BDA00026757180800001011
αθfor the installation deviation angle, K, between strapdown inertial navigation and intelligent tunnelling robotDThe oil cylinder shift coefficient error is obtained;
s303, a lever arm error model is as follows:
Figure BDA0002675718080000111
in the formula, l is a position vector of the oil cylinder relative to the strapdown inertial navigation.
S4, fusing the data of the strapdown inertial navigation and the oil cylinder through a standard Kalman filtering algorithm, specifically as follows:
s401, comprehensively considering strapdown inertial navigation errors, dead reckoning errors and lever arm errors, and establishing a system state equation of the strapdown inertial navigation and an oil cylinder;
wherein, the state variables of the system are as follows:
Figure BDA0002675718080000116
in the formula ,φTTo calculate the misalignment angle error between the navigation coordinate system and the ideal navigation coordinate system, (v)TFor the speed error of the intelligent tunneling robot, (p)TPosition error for intelligent tunneling robot, (p)D)TFor dead reckoning position error, (p)GL)TFor lever arm error: (b)TThe error is the zero offset error of the gyroscope,
Figure BDA0002675718080000112
for the zero offset error of the accelerometer,
Figure BDA0002675718080000113
and l is the position vector of the strapdown inertial navigation and the oil cylinder. The state equation of the combined measurement system is as follows:
Figure BDA0002675718080000114
wherein ,
Figure BDA0002675718080000115
Figure BDA0002675718080000121
Figure BDA0002675718080000122
and
Figure BDA0002675718080000123
white noise for measuring the angular velocity of the gyroscope and white noise for measuring the specific force of the accelerometer respectively;
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 oil cylinder
The difference between the position resolved by strapdown inertial navigation and the position of dead reckoning is used to construct an observation vector, i.e.
Z＝p-pD
The measurement equation of the system is
Z＝HX+V
Wherein H ═ O3×6 I3×3 -I3×3 O3×15]V is measurement noise;
s403, establishing a strapdown inertial navigation and oil cylinder standard Kalman filtering equation to obtain an optimal estimation value of the system state, wherein the iteration process can be divided into the following 5 steps
1) One-step prediction is carried out on the current time state through the state equation and the state of the last time 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 k-1.
2) Solving the mean square error matrix of the error of the prediction state at the current moment
P(k/k-1)＝FP(k-1)FT+Q
P (k-1) is a mean square error matrix of the system state error at the k-1 moment, and Q is a system noise matrix.
3) Solving Kalman gain K
Kk＝P(k/k-1)HT(HP(k/k-1)HT+R)-1
Wherein R is a measurement noise matrix
4) Updating the optimal estimation value of the current time state by combining Kalman gain K
X(k)＝X(k/k-1)+Kk(Zk-HX(k/k-1))
5) Updating the mean square error matrix of the optimal estimation value error of the current time state
P(k)＝(I-KkH)P(k/k-1)
And S6, obtaining a pose curve of the intelligent tunneling robot according to the fusion result of the displacement data of the intelligent tunneling robot pushed by the oil cylinder and the strapdown inertial navigation data, and realizing accurate positioning and orientation of the tunneling working face.
The foregoing description of specific embodiments of the present invention has been presented. It is to be understood that the present invention is not limited to the specific embodiments described above, and that various changes or modifications may be made by one skilled in the art within the scope of the appended claims without departing from the spirit of the invention. The embodiments and features of the embodiments of the present application may be combined with each other arbitrarily without conflict.

Claims (6)

1. The intelligent tunneling robot positioning and orienting method based on the strapdown inertial navigation and the oil cylinder is characterized in that correction of pose data of the intelligent tunneling robot solved by the strapdown inertial navigation is achieved based on displacement data of the intelligent tunneling robot pushed by the oil cylinder to advance, and therefore high-precision positioning and orienting of the intelligent tunneling robot is achieved.
2. The intelligent tunneling robot positioning and orienting method based on the strapdown inertial navigation and the oil cylinder is characterized by comprising the following steps of:
s1, installing the strapdown inertial navigation system on the 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 the positive direction of a Y axis, taking the direction perpendicular to the Y axis on the right side of the intelligent tunneling robot as the positive direction of an X axis, taking the upward direction perpendicular to the intelligent tunneling robot as the positive direction of a Z axis, and establishing a carrier coordinate system OXYZ;
s102, taking an east-north-sky coordinate system of the position where the strapdown inertial navigation device is arranged as a navigation coordinate system O1X1Y1Z1；
S103, under a carrier coordinate system, the angle of rotation of the intelligent tunneling robot around the Z axis is called a course angle and is recorded as a course angle
Figure FDA0002675718070000012
Angle of rotation about the X-axisThe attitude matrix from the navigation coordinate system to the carrier coordinate system of the intelligent tunneling robot is as follows:
Figure FDA0002675718070000011
s2, forming a dead reckoning system by the strapdown inertial navigation and the oil cylinder, and analyzing errors contained in the dead reckoning system;
s201, constructing a strapdown inertial navigation error model:
Figure FDA0002675718070000021
Figure FDA0002675718070000022
Figure FDA0002675718070000023
Figure FDA0002675718070000024
Figure FDA0002675718070000025
Figure FDA0002675718070000026
Figure FDA0002675718070000027
Figure FDA0002675718070000028
Figure FDA0002675718070000029
in the formula ,φE、φN、φURespectively, for calculating the misalignment angle error, omega, between the navigation coordinate system and the ideal navigation coordinate systemE、ωN、ωURespectively, measured values of a gyroscope, vE、vN、vURespectively the speed of the intelligent tunneling robot in the navigation coordinate system,
Figure FDA00026757180700000210
respectively the speed error R of the intelligent tunneling robot in the navigation coordinate systemNh、RMhRespectively a meridian radius and a unit radius, fE、fN、fURespectively, accelerometer measurements, L is the local latitude, L, λ, h are latitude, longitude and altitude errors, respectively,E、N、Urespectively, are the zero-offset errors of the gyroscope,
Figure FDA00026757180700000211
respectively, the zero offset error, g, of the accelerometereIs equatorial gravity, beta1、β20.005302, 3.08X 10 respectively-6、8.08×10-9；
S202, constructing a dead reckoning position error model:
Figure FDA00026757180700000212
Figure FDA0002675718070000031
Figure FDA0002675718070000032
in the formula ,LD、λD、hDRespectively is latitude, longitude and altitude V of the position of the intelligent tunneling robot during dead reckoningDE、VDN、VDURespectively an east speed, a north speed and a sky speed of the intelligent tunneling robot during dead reckoning,
Figure FDA0002675718070000033
αθfor the installation deviation angle, K, between strapdown inertial navigation and intelligent tunnelling robotDThe oil cylinder shift coefficient error is obtained;
s203, constructing a lever arm error model:
Figure FDA0002675718070000034
in the formula, l is a position vector of the oil cylinder relative to the strapdown inertial navigation;
s3, fusing data of the strapdown inertial navigation and the oil cylinder through a standard Kalman filtering algorithm;
s301, comprehensively considering strapdown inertial navigation errors, dead reckoning errors and lever arm errors, and establishing a system state equation of the strapdown inertial navigation and the oil cylinder;
wherein, the state variables of the system are as follows:
Figure FDA0002675718070000035
in the formula ,φTTo calculate the misalignment angle error between the navigation coordinate system and the ideal navigation coordinate system, (v)TFor the speed error of the intelligent tunneling robot, (p)TPosition error for intelligent tunneling robot, (p)D)TAs the position during dead reckoningError (p)GL)TFor lever arm error: (b)TThe error is the zero offset error of the gyroscope,
Figure FDA0002675718070000036
for the zero offset error of the accelerometer,
Figure FDA0002675718070000037
l is the position vector of the strapdown inertial navigation and the oil cylinder;
the state equation of the combined measurement system is as follows:
Figure FDA0002675718070000038
wherein ,
Figure FDA0002675718070000041
Figure FDA0002675718070000042
Figure FDA0002675718070000043
and
Figure FDA0002675718070000044
white noise for measuring the angular velocity of the gyroscope and white noise for measuring the specific force of the accelerometer respectively;
according to the error model in S201, S202, S203, the parameters in the state transition matrix F are as follows:
Figure FDA0002675718070000045
Figure FDA0002675718070000046
Figure FDA0002675718070000047
Figure FDA0002675718070000048
Figure FDA0002675718070000049
Figure FDA00026757180700000410
Figure FDA0002675718070000051
s302, establishing a system measurement equation of strapdown inertial navigation and oil cylinder
The difference between the position resolved by strapdown inertial navigation and the position of dead reckoning is used to construct an observation vector, i.e.
Z＝p-pD
The measurement equation of the system is
Z＝HX+V
Wherein H is [0 ]3×6 I3×3 -I3×3 03×15]V is measurement noise;
s303, establishing a strapdown inertial navigation and oil cylinder standard Kalman filtering equation to obtain an optimal estimation value of the system state, wherein the iteration process can be divided into the following 5 steps
1) One-step prediction is carried out on the current time state through the state equation and the state of the last time of the system
X(k/k-1)＝F·X(k-1)
Wherein X (k-1) is the state quantity of the system at the k-1 moment;
2) solving the mean square error matrix of the error of the prediction state at the current moment
P(k/k-1)＝FP(k-1)FT+Q
P (k-1) is a mean square error matrix of the system state error at the k-1 moment, and Q is a system noise matrix;
3) solving Kalman gain K
Kk＝P(k/k-1)HT(HP(k/k-1)HT+R)-1
Wherein, R is a measurement noise matrix;
4) updating the optimal estimation value of the current time state by combining Kalman gain K
X(k)＝X(k/k-1)+Kk(Zk-HX(k/k-1))
5) Updating the mean square error matrix of the optimal estimation value error of the current time state
P(k)＝(I-KkH)P(k/k-1)
And S4, obtaining a pose curve of the intelligent tunneling robot according to the fusion result of the displacement data of the intelligent tunneling robot pushed by the oil cylinder and the strapdown inertial navigation data, and realizing accurate positioning and orientation of the tunneling working face.
3. The intelligent tunneling robot positioning and orienting method based on the strapdown inertial navigation and the oil cylinder as claimed in claim 1, wherein: when the intelligent tunneling robot moves forwards linearly, the oil cylinder displacement amount of the left side and the right side of the intelligent tunneling robot is equal, and at the moment, the displacement amount of the intelligent tunneling robot is equal to the oil cylinder displacement amount; when the intelligent tunneling robot corrects the deviation, the oil cylinder pushing amount on the left side and the right side is unequal, and the strapdown inertial navigation is fixedly connected with the robot I, so that 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 I of the robot, and the displacement data of the intelligent tunneling robot is as follows:
Figure FDA0002675718070000061
wherein a is the distance between the strapdown inertial navigation projection and the left surface of the robot shield I, b is the distance between the strapdown inertial navigation projection and the right surface of the robot shield I, and LlFor the displacement, L, of the left side oil cylinder of the intelligent tunneling robotrThe intelligent tunneling robot is the displacement of the oil cylinder on the right side.
4. The intelligent tunneling robot positioning and orienting method based on the strapdown inertial navigation and the oil cylinder as claimed in claim 1, wherein: and combining the attitude data of the intelligent tunneling robot obtained by the strapdown inertial navigation solution 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.
5. The intelligent tunneling robot positioning and orienting method based on the strapdown inertial navigation and the oil cylinder as claimed in claim 1, wherein: and (3) considering lever arm errors generated by different oil cylinder positions and strapdown inertial navigation installation positions, and constructing a lever arm error model.
6. The intelligent tunneling robot positioning and orienting method based on the strapdown inertial navigation and the oil cylinder as claimed in claim 1, wherein: 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 the 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 true CN112033401A (en) 2020-12-04
CN112033401B 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 (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20050197755A1 (en) * 2004-03-05 2005-09-08 Knowlton Timothy J. Multi-source positioning system for work machines
JP2007263160A (en) * 2006-03-27 2007-10-11 Kayaba Ind Co Ltd Energy conversion device
US20130158789A1 (en) * 2011-03-24 2013-06-20 Masanobu Seki Hydraulic shovel calibration device and hydraulic shovel calibration method
US20150330060A1 (en) * 2014-05-14 2015-11-19 Komatsu Ltd. Calibration system and calibration method for excavator
CN107270901A (en) * 2017-08-17 2017-10-20 中国矿业大学 Merge the coal-winning machine accuracy of inertial position method for improving of coal winning technology and coal-winning machine motion model
US20180372498A1 (en) * 2017-06-21 2018-12-27 Caterpillar Inc. System and method for determining machine state using sensor fusion
CN109540130A (en) * 2018-10-25 2019-03-29 北京航空航天大学 A kind of continuous milling machine inertial navigation positioning and orienting method
CN110095135A (en) * 2019-06-03 2019-08-06 中南大学 A kind of method and device for development machine positioning and directing
CN110162036A (en) * 2019-04-09 2019-08-23 中国矿业大学 A kind of development machine Camera calibration system and method
CN110996048A (en) * 2019-11-20 2020-04-10 中国煤炭科工集团太原研究院有限公司 Remote visualization system and method for coal roadway heading machine

Patent Citations (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20050197755A1 (en) * 2004-03-05 2005-09-08 Knowlton Timothy J. Multi-source positioning system for work machines
JP2007263160A (en) * 2006-03-27 2007-10-11 Kayaba Ind Co Ltd Energy conversion device
US20130158789A1 (en) * 2011-03-24 2013-06-20 Masanobu Seki Hydraulic shovel calibration device and hydraulic shovel calibration method
US20150330060A1 (en) * 2014-05-14 2015-11-19 Komatsu Ltd. Calibration system and calibration method for excavator
US20180372498A1 (en) * 2017-06-21 2018-12-27 Caterpillar Inc. System and method for determining machine state using sensor fusion
CN109115213A (en) * 2017-06-21 2019-01-01 卡特彼勒公司 For merging the system and method to determine machine state using sensor
CN107270901A (en) * 2017-08-17 2017-10-20 中国矿业大学 Merge the coal-winning machine accuracy of inertial position method for improving of coal winning technology and coal-winning 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
CN110095135A (en) * 2019-06-03 2019-08-06 中南大学 A kind of method and device for development machine positioning and directing
CN110996048A (en) * 2019-11-20 2020-04-10 中国煤炭科工集团太原研究院有限公司 Remote visualization system and method for coal roadway heading machine

Non-Patent Citations (5)

* Cited by examiner, † Cited by third party
Title
LI NING; DING WEI: "Model-Aided Strapdown Inertial Navigation Integrated Method for AUV Based on H8 Filtering", 2013 INTERNATIONAL CONFERENCE ON COMPUTATIONAL AND INFORMATION SCIENCES *
石涛: "超大采高工作面大断面巷道超前支护设备研制与应用", 《煤矿机械》 *
石涛: "超大采高工作面大断面巷道超前支护设备研制与应用", 《煤矿机械》, vol. 41, no. 04, 30 April 2020 (2020-04-30), pages 143 - 145 *
郝尚清;李昂;王世博;等: "采煤机惯性导航安装偏差对定位误差的影响", 煤炭学报, vol. 40, no. 08 *
黎永键;赵祚喜;高俊文;吴晓鹏;关伟;: "基于DGPS定位与双闭环转向控制的农业自动导航系统", 农业现代化研究, vol. 37, no. 02 *

Also Published As

Publication number Publication date
CN112033401B (en) 2023-06-20

Similar Documents

Publication Publication Date Title
CN103424114B (en) A kind of full combined method of vision guided navigation/inertial navigation
CN110487301B (en) Initial alignment method of radar-assisted airborne strapdown inertial navigation system
CN110926468B (en) Communication-in-motion antenna multi-platform navigation attitude determination method based on transfer alignment
CN112097763B (en) Underwater vehicle combined navigation method based on MEMS IMU/magnetometer/DVL combination
CN112378399B (en) Coal mine tunnel tunneling robot precise positioning and orientation method based on strapdown inertial navigation and digital total station
CN110702109B (en) Coal mining machine inertial navigation/wireless sensor network combined positioning method
CN111207744B (en) Pipeline geographical position information measuring method based on thick tail robust filtering
CN111637888B (en) Tunneling machine positioning method and system based on inertial navigation and laser radar single-point distance measurement
CN104515527A (en) Anti-rough error integrated navigation method under non-GPS signal environment
CN114701544B (en) Method and system for accurately positioning multi-source information fusion of underground monorail crane of coal mine
CN111207742B (en) Coal mining machine positioning and attitude determining method with additional external orientation element constraint
CN115773751A (en) Method for correcting alignment error caused by zero position of equivalent antenna direction adder
CN114148536B (en) Linear displacement compensation control method for photoelectric pod
CN111323011A (en) Coal mining machine body and rocker arm cooperative positioning device and positioning method
Sun et al. A vehicle-carried INS positioning accuracy improvement method by using lateral constraint in GPS-denied environment
CN113236363A (en) Mining equipment navigation positioning method, system, equipment and readable storage medium
CN112781588B (en) Navigation resolving method for while-drilling gyroscope positioning and orientation instrument
CN112729332B (en) Alignment method based on rotation modulation
CN111504313B (en) Drilling positioning and orientating instrument hole positioning method based on speed information assistance
CN111189446A (en) Radio-based integrated navigation method
CN111207734B (en) EKF-based unmanned aerial vehicle integrated navigation method
CN111220151B (en) Inertia and milemeter combined navigation method considering temperature model under load system
CN112033401A (en) Intelligent tunneling robot positioning and orienting method based on strapdown inertial navigation and oil cylinder
CN110864688A (en) Attitude heading method for vehicle-mounted azimuth open-loop horizontal attitude angle closed loop
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