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

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

Positioning and Orientation Method of Intelligent Roadheading Robot Based on Strapdown Inertial Navigation and Oil Cylinder

technical field

The invention relates to the technical field of coal mining, in particular to a method for positioning and orienting an intelligent excavation robot based on strapdown inertial navigation and an oil cylinder.

Background technique

As an important part of my country's energy, coal will maintain its dominant position for a long time to come. With the continuous improvement of scientific and technological innovation capabilities, the intelligent level of coal mining has been greatly improved, and the mining efficiency and safety have been guaranteed. In the current coal mining process, the precise positioning and orientation of the roadheader is an important research direction for the intelligentization of the fully mechanized face. At this stage, the driving direction of the roadheader is mostly guided by lasers. This method cannot measure the attitude information of the roadheader, which leads to the inability to provide attitude information for the automatic correction of the roadheader's driving direction. In the process of coal mines developing towards intelligence , this method can no longer meet the current needs.

Due to the complexity of the underground environment, some pose measurement methods that can be used on the ground are not suitable for the underground. However, inertial navigation can work in various environments on the ground, underground and in the air around the clock without relying on external information. The first choice for positioning and orientation of equipment in coal mines. However, the measurement error of pure inertial navigation measurement system will increase with time.

SUMMARY OF THE INVENTION

In view of the above problems, the patent of the present invention provides a method for positioning and orienting an intelligent excavation robot based on strapdown inertial navigation and an oil cylinder.

To achieve the above object, the technical scheme adopted in the present invention is:

A method of positioning and orienting an intelligent excavation robot based on strapdown inertial navigation and an oil cylinder. Based on the displacement data of the intelligent excavation robot moving forward by the oil cylinder, it realizes the correction of the pose data of the intelligent excavation robot calculated by the strapdown inertial navigation, so as to realize the intelligent excavation robot. High-precision positioning and orientation, among which, the intelligent excavation robot is composed of robot I and robot II. The connection between robot I and robot II is through the oil cylinder. When the intelligent excavation robot moves forward, it first pushes robot I to the predetermined position through the oil cylinder, and then in Pull the robot II to the predetermined position through the oil cylinder. Specifically, it includes the following steps:

S1, install the strapdown inertial navigation on the robot I, and establish the carrier coordinate system and the navigation coordinate system;

S101, install the strapdown inertial navigation on the robot I, take the center of gravity of the intelligent excavation robot as the coordinate origin, the forward direction of the intelligent excavation robot is the positive direction of the Y axis, and the right side of the intelligent excavation robot perpendicular to the Y axis is the positive direction of the X axis, The upward direction of the vertical intelligent excavation robot is used as the positive direction of the Z axis, and the carrier coordinate system OXYZ is established;

S102, using the "east-north-sky" coordinate system at the location where the strapdown inertial navigation is installed as the navigation coordinate system O ₁ X ₁ Y ₁ Z ₁ ;

绕X轴旋转的角度称为俯仰角，记为θ，绕Y轴旋转的角度称为横滚角，记为γ，则智能掘进机器人从导航坐标系到载体坐标系的姿态矩阵如下：

S103. In the carrier coordinate system, the rotation angle of the intelligent excavation robot around the Z axis is called the heading angle, and is recorded as

The angle rotated around the X axis is called the pitch angle, denoted as θ, and the angle rotated around the Y axis is called the roll angle, denoted as γ, then the attitude matrix of the intelligent excavation robot from the navigation coordinate system to the carrier coordinate system is as follows:

S2. Form a dead reckoning system with strapdown inertial navigation and oil cylinder, and analyze the errors contained in it;

S201. Build a strapdown inertial navigation error model:

分别为智能掘进机器人在导航坐标系中的速度误差，R_Nh、R_Mh分别为子午圈半径和卯酉圈半径，f_E、f_N、f_U分别为加速度计测量值，L为当地的纬度.δL、δλ、δh分别为纬度、经度和高度误差，ε_E、ε_N、ε_U分别为陀螺仪零偏误差，

分别为加速度计零偏误差，g_e为赤道重力，β、β₁、β₂分别为0.005302、3.08×10^-6、8.08×10^-9；In the formula, φ _E , φ _N , and φ _U are the misalignment angle errors between the calculated navigation coordinate system and the ideal navigation coordinate system, respectively, ω _E , ω _N , and ω _U are the measured values of the gyroscope, respectively, v _E , v _N and v _U are the speeds of the intelligent excavation robot in the navigation coordinate system, respectively,

are the speed errors of the intelligent excavation robot in the navigation coordinate system, respectively, R _Nh and R _Mh are the radius of the meridian circle and the radius of the unitary circle, respectively, f _E , f _N , and f _U are the measured values of the accelerometer, and L is the local latitude .δL, δλ, δh are the latitude, longitude and altitude errors, respectively, ε _E , ε _N , ε _U are the gyroscope bias errors, respectively,

are the zero bias error of the accelerometer, g _e is the equatorial gravity, and β, β ₁ , and β ₂ are 0.005302, 3.08×10 ^-6 , and 8.08×10 ^-9 respectively;

S202. Build a position error model for dead reckoning:

α_θ为捷联惯导和智能掘进机器人之间的安装偏差角，δK_D为油缸推移系数误差；In the formula, L _D , λ _D , and h _D are the latitude, longitude and altitude of the location of the intelligent excavation robot during dead reckoning, respectively, and V _DE , V _DN , and V _DU are the eastward speed of the intelligent excavation robot in dead reckoning, respectively. , the north speed and the sky speed,

α _θ is the installation deviation angle between the strapdown inertial navigation and the intelligent excavation robot, and δK _D is the cylinder displacement coefficient error;

S203. Build a lever arm error model:

In the formula, δl is the position vector of the cylinder relative to the strapdown inertial navigation;

S3. Through the standard Kalman filtering algorithm, the data fusion of the strapdown inertial navigation and the oil cylinder is realized;

S301, comprehensively consider the strapdown inertial navigation error, dead reckoning error and lever arm error, and establish the system state equation of the strapdown inertial navigation and oil cylinder;

Among them, the state variables of the system are:

为加速度计零偏误差，

l为捷联惯导和油缸的位置矢量；In the formula, φ ^T is the misalignment angle error between the calculated navigation coordinate system and the ideal navigation coordinate system, (δv) ^T is the speed error of the intelligent excavation robot, (δp) ^T is the position error of the intelligent excavation robot, (δp _D )T is the position error during dead reckoning, (δp _GL ) ^T is the lever arm error, (ε ^b ) ^T is the gyroscope bias error,

is the zero bias error of the accelerometer,

l is the position vector of strapdown inertial navigation and oil cylinder;

The state equation of the combined measurement system is:

in,

和

分别为陀螺角速度测量白噪声和加速度计比力测量白噪声；

and

are the white noise of gyro angular velocity measurement and the white noise of accelerometer specific force measurement;

According to the error model in S201, S202, S203, the parameters in the state transition matrix F are as follows:

S302. Establish the system measurement equation of strapdown inertial navigation and oil cylinder

The observation vector is constructed by the difference between the position calculated by the strapdown inertial navigation and the position calculated by the dead reckoning, namely

Z=δp-δp _D

Then 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 the measurement noise;

S303, establish the strapdown inertial navigation and oil cylinder standard Kalman filter equation to obtain the optimal estimated value of the system state, and the iterative process can be divided into the following five steps

1) One-step prediction of the state at the current moment through the state equation and the state of the system at the previous moment

X(k/k-1)=F·X(k-1)

where X(k-1) is the state quantity of the system at time k-1;

2) Solve the mean square error matrix of the error of the predicted state at the current moment

P(k/k-1)=FP(k-1)F ^T +Q

P(k-1) is the mean square error matrix of the system state error at time k-1, and Q is the system noise matrix;

3) Solve the 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) Combine the Kalman gain K to update the optimal estimated value of the state at the current moment

X(k)=X(k/k-1)+ _Kk ( _Zk -HX(k/k-1))

5) Update the mean square error matrix of the optimal estimated value error of the state at the current moment

P(k)=(IK _k H)P(k/k-1)

S4. According to the fusion result of the forward displacement data of the cylinder-moving intelligent excavation robot and the strapdown inertial navigation data, the pose curve of the intelligent excavation robot is obtained, so as to realize the precise positioning and orientation of the excavation face.

Further, when the intelligent excavation robot moves straight forward, the displacement of the oil cylinders on the left and right sides of the intelligent excavation robot is equal, and at this time, the displacement of the intelligent excavation robot is equal to the displacement of the oil cylinder; Since the SINS is fixedly connected to the robot I, the displacement of the robot I is equivalent to the displacement of the SINS, and the displacement of the SINS can be approximately equivalent to the projected displacement behind the shield of the robot I. This The displacement data of the intelligent excavation robot displacement is as follows:

In the formula, a is the distance between the SINS projection and the left surface of the robot I shield, b is the distance between the SINS projection and the right surface of the robot I shield, L _l is the displacement of the left oil cylinder of the intelligent excavation robot, L _r is the displacement of the right oil cylinder of the intelligent excavation robot.

Further, the attitude data of the intelligent excavation robot obtained by the strapdown inertial navigation solution and the forward displacement data of the cylinder-moving intelligent excavation robot are combined to form a dead reckoning system, and an error model of the dead reckoning system is constructed.

Further, the lever-arm error caused by the difference between the position of the oil cylinder and the installation position of the strapdown inertial navigation system is considered, and the lever-arm error model is constructed.

Further, considering the error of the strapdown inertial navigation system, the error of the dead reckoning system and the lever arm error, the standard Kalman filter is used to realize the fusion of the strapdown inertial navigation data and the oil cylinder data, and the high-precision positioning and orientation of the intelligent roadheading robot is obtained. data.

The present invention has the following beneficial effects:

(1) Compensate the accumulated error of the strapdown inertial navigation by using the displacement data of the intelligent excavation robot moving forward by the cylinder, and overcome the disadvantage that the positioning and orientation error of the intelligent excavation robot will diverge with time due to the pure inertial navigation solution;

(2) It can adapt to the complex underground environment of coal mines and realize the autonomous measurement of the pose data of the intelligent excavation robot.

Description of drawings

FIG. 1 is a schematic diagram of the present invention.

Fig. 2 is the installation schematic diagram 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 deviation correction of the present invention.

FIG. 4 is a coordinate system diagram of the present invention.

Figure 5 is a flow chart of the present invention.

Detailed ways

The present invention will be described in detail below with reference to specific embodiments. The following examples will help those skilled in the art to further understand the present invention, but do not limit the present invention in any form. It should be noted that, for those skilled in the art, several modifications and improvements can be made without departing from the concept of the present invention. These all belong to the protection scope of the present invention.

An embodiment of the present invention provides a method for positioning and orienting an intelligent excavation robot based on strapdown inertial navigation and an oil cylinder. The intelligent excavation robot is composed of a robot I and a robot II. When the robot moves forward, firstly, the robot I is pushed to the predetermined position by the oil cylinder, and then the robot II is pulled to the predetermined position by the oil cylinder. The positioning and orientation method includes the following steps:

S1, install the strapdown inertial navigation on the robot I, and establish the carrier coordinate system and the navigation coordinate system;

S101. Take the center of gravity of the intelligent excavation robot as the coordinate origin, the forward direction of the intelligent excavation robot is the positive direction of the Y axis, the right side of the intelligent excavation robot perpendicular to the Y axis is the positive direction of the X axis, and the upward direction of the vertical intelligent excavation robot is the positive direction of the Z axis , establish the carrier coordinate system OXYZ;

S102, using the "east-north-sky" coordinate system at the location where the strapdown inertial navigation is installed as the navigation coordinate system O ₁ X ₁ Y ₁ Z ₁ ;

绕X轴旋转的角度称为俯仰角，记为θ，绕Y轴旋转的角度称为横滚角，记为γ，则智能掘进机器人从导航坐标系到载体坐标系的姿态矩阵如下：

S103. In the carrier coordinate system, the rotation angle of the intelligent excavation robot around the Z axis is called the heading angle, and is recorded as

The angle rotated around the X axis is called the pitch angle, denoted as θ, and the angle rotated around the Y axis is called the roll angle, denoted as γ, then the attitude matrix of the intelligent excavation robot from the navigation coordinate system to the carrier coordinate system is as follows:

S2. When the intelligent excavation robot moves in a straight line, the displacement of the left and right cylinders of the intelligent excavation robot is equal. At this time, the displacement of the intelligent excavation robot is equal to the displacement of the oil cylinder; when the intelligent excavation robot corrects the deviation, the displacement of the left and right oil cylinders is not equal. The SINS is fixedly connected to the robot I, so the displacement of the robot I is equivalent to the displacement of the SINS, and the displacement of the SINS can be approximately equivalent to the projected displacement behind the shield of the robot I (see Fig. 3), the mathematical model of the displacement of the intelligent excavation robot is as follows

In the formula, a is the distance between the SINS projection and the left surface of the robot I shield, b is the distance between the SINS projection and the right surface of the robot I shield, L _l is the displacement of the left oil cylinder of the intelligent excavation robot, L _r is the displacement of the right oil cylinder of the intelligent excavation robot;

S3. The dead reckoning system is formed by the strapdown inertial navigation and the oil cylinder, and the errors contained in it are analyzed. The specific results are as follows:

S301, strapdown inertial navigation error model:

分别为智能掘进机器人在导航坐标系中的速度误差，R_Nh、R_Mh分别为子午圈半径和卯酉圈半径，f_E、f_N、f_U分别为加速度计测量值，L为当地的纬度，δL、δλ、δh分别为纬度、经度和高度误差，ε_E、ε_N、ε_U分别为陀螺仪零偏误差，

分别为加速度计零偏误差，g_e为赤道重力，β、β₁、β₂分别为0.005302、3.08×10^-6、8.08×10^-9；In the formula, φ _E , φ _N , and φ _U are the misalignment angle errors between the calculated navigation coordinate system and the ideal navigation coordinate system, respectively, ω _E , ω _N , and ω _U are the measured values of the gyroscope, respectively, v _E , v _N and v _U are the speeds of the intelligent excavation robot in the navigation coordinate system, respectively,

are the speed errors of the intelligent excavation robot in the navigation coordinate system, respectively, R _Nh and R _Mh are the radius of the meridian circle and the radius of the unitary circle, respectively, f _E , f _N , and f _U are the measured values of the accelerometer, and L is the local latitude , δL, δλ, δh are the latitude, longitude and altitude errors, respectively, ε _E , ε _N , ε _U are the gyroscope bias errors, respectively,

are the zero bias error of the accelerometer, g _e is the equatorial gravity, and β, β ₁ , and β ₂ are 0.005302, 3.08×10 ^-6 , and 8.08×10 ^-9 respectively;

S302. The position error model of dead reckoning is as follows:

α_θ为捷联惯导和智能掘进机器人之间的安装偏差角，δK_D为油缸推移系数误差；In the formula, L _D , λ _D , and h _D are the latitude, longitude and altitude of the location of the intelligent excavation robot during dead reckoning, respectively, and V _DE , V _DN , and V _DU are the eastward speed of the intelligent excavation robot in dead reckoning, respectively. , the north speed and the sky speed,

α _θ is the installation deviation angle between the strapdown inertial navigation and the intelligent excavation robot, and δK _D is the cylinder displacement coefficient error;

S303, the lever arm error model is as follows:

In the formula, δl is the position vector of the cylinder relative to the SINS.

S4. Integrate the data of the strapdown inertial navigation and the oil cylinder through the standard Kalman filtering algorithm, as follows:

S401, comprehensively consider the strapdown inertial navigation error, dead reckoning error and lever arm error, establish the system state equation of the strapdown inertial navigation and the oil cylinder;

Among them, the state variables of the system are:

为加速度计零偏误差，

l为捷联惯导和油缸的位置矢量。组合测量系统的状态方程为：In the formula, φ ^T is the misalignment angle error between the calculated navigation coordinate system and the ideal navigation coordinate system, (δv) ^T is the speed error of the intelligent excavation robot, (δp) ^T is the position error of the intelligent excavation robot, (δp _D ) ^T is the position error during dead reckoning, (δp _GL ) ^T is the lever arm error, (ε ^b ) ^T is the gyroscope bias error,

is the zero bias error of the accelerometer,

l is the position vector of strapdown inertial navigation and oil cylinder. The state equation of the combined measurement system is:

in,

和

分别为陀螺角速度测量白噪声和加速度计比力测量白噪声；

and

are the white noise of gyro angular velocity measurement and the white noise of accelerometer specific force measurement;

According to the error model in S201, S202, S203, the parameters in the state transition matrix F are as follows:

S402. Establish the system measurement equation of strapdown inertial navigation and oil cylinder

The observation vector is constructed by the difference between the position calculated by the strapdown inertial navigation and the position calculated by the dead reckoning, namely

Z=δp-δp _D

Then 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 the measurement noise;

S403, establish the strapdown inertial navigation and the standard Kalman filter equation of the oil cylinder, and obtain the optimal estimated value of the system state, and the iterative process can be divided into the following 5 steps

1) One-step prediction of the state at the current moment through the state equation and the state of the system at the previous moment

X(k/k-1)=F·X(k-1)

where X(k-1) is the state quantity of the system at time k-1.

2) Solve the mean square error matrix of the error of the predicted state at the current moment

P(k/k-1)=FP(k-1)F ^T +Q

P(k-1) is the mean square error matrix of the system state error at time k-1, and Q is the system noise matrix.

3) Solve the Kalman gain K

K _k =P(k/k-1)H ^T (HP(k/k-1)H ^T +R) ^-1

where R is the measurement noise matrix

4) Combine the Kalman gain K to update the optimal estimated value of the state at the current moment

X(k)=X(k/k-1)+ _Kk ( _Zk -HX(k/k-1))

5) Update the mean square error matrix of the optimal estimated value error of the state at the current moment

P(k)=(IK _k H)P(k/k-1)

S6. According to the fusion result of the forward displacement data of the cylinder-moving intelligent excavation robot and the strapdown inertial navigation data, the pose curve of the intelligent excavation robot is obtained, so as to realize the precise positioning and orientation of the excavation face.

Specific embodiments of the present invention have been described above. It should be understood that the present invention is not limited to the above-mentioned specific embodiments, and those skilled in the art can make various changes or modifications within the scope of the claims, which do not affect the essential content of the present invention. The embodiments of the present application and features in the embodiments may be combined with each other arbitrarily, provided that there is no conflict.

Claims (6)

1. a kind of intelligent excavation robot positioning and orientation method based on strapdown inertial navigation and oil cylinder, it is characterized in that, realize the correction of the pose data of the intelligent excavation robot that the strapdown inertial navigation solution calculates based on the displacement data that the oil cylinder pushes the intelligent excavation robot forward , so as to realize the high-precision positioning and orientation of the intelligent excavation robot. The intelligent excavation robot is composed of robot I and robot II, and the oil cylinder is connected between the robot I and the robot II. When the intelligent excavation robot moves forward, it first pushes the robot I through the oil cylinder. to the predetermined position, and then pull the robot II to the predetermined position through the cylinder. 2. a kind of intelligent driving robot positioning and orientation method based on strapdown inertial navigation and oil cylinder as claimed in claim 1, is characterized in that, comprises the following steps: S1, install the strapdown inertial navigation on the robot I, and establish the carrier coordinate system and the navigation coordinate system; S101, install the strapdown inertial navigation on the robot I, take the center of gravity of the intelligent excavation robot as the coordinate origin, the forward direction of the intelligent excavation robot is the positive direction of the Y axis, and the right side of the intelligent excavation robot perpendicular to the Y axis is the positive direction of the X axis, The upward direction of the vertical intelligent excavation robot is used as the positive direction of the Z axis, and the carrier coordinate system OXYZ is established; S102, using the "east-north-sky" coordinate system at the location where the strapdown inertial navigation is installed as the navigation coordinate system O ₁ X ₁ Y ₁ Z ₁ ; S103. In the carrier coordinate system, the rotation angle of the intelligent excavation robot around the Z axis is called the heading angle, and is recorded as

The angle rotated around the X axis is called the pitch angle, denoted as θ, and the angle rotated around the Y axis is called the roll angle, denoted as γ, then the attitude matrix of the intelligent excavation robot from the navigation coordinate system to the carrier coordinate system is as follows:

S2. Form a dead reckoning system with strapdown inertial navigation and oil cylinder, and analyze the errors contained in it; S201. Build a strapdown inertial navigation error model:

In the formula, φ _E , φ _N , and φ _U are the misalignment angle errors between the calculated navigation coordinate system and the ideal navigation coordinate system, respectively, ω _E , ω _N , and ω _U are the measured values of the gyroscope, respectively, v _E , v _N and v _U are the speeds of the intelligent excavation robot in the navigation coordinate system, respectively,

are the speed errors of the intelligent excavation robot in the navigation coordinate system, respectively, R _Nh and R _Mh are the radius of the meridian circle and the radius of the unitary circle, respectively, f _E , f _N , and f _U are the measured values of the accelerometer, and L is the local latitude , δL, δλ, δh are the latitude, longitude and altitude errors, respectively, ε _E , ε _N , ε _U are the gyroscope bias errors, respectively,

are the zero bias error of the accelerometer, g _e is the equatorial gravity, and β, β ₁ , and β ₂ are 0.005302, 3.08×10 ^-6 , and 8.08×10 ^-9 respectively; S202. Build a position error model for dead reckoning:

In the formula, L _D , λ _D , and h _D are the latitude, longitude and altitude of the location of the intelligent excavation robot during dead reckoning, respectively, and V _DE , V _DN , and V _DU are the eastward speed of the intelligent excavation robot in dead reckoning, respectively. , the north speed and the sky speed,

α _θ is the installation deviation angle between the strapdown inertial navigation and the intelligent excavation robot, and δK _D is the cylinder displacement coefficient error; S203. Build a lever arm error model:

In the formula, δl is the position vector of the cylinder relative to the strapdown inertial navigation; S3. Through the standard Kalman filtering algorithm, the data fusion of the strapdown inertial navigation and the oil cylinder is realized; S301, comprehensively consider the strapdown inertial navigation error, dead reckoning error and lever arm error, establish the system state equation of the strapdown inertial navigation and oil cylinder; Among them, the state variables of the system are:

In the formula, φ ^T is the misalignment angle error between the calculated navigation coordinate system and the ideal navigation coordinate system, (δv) ^T is the speed error of the intelligent excavation robot, (δp) ^T is the position error of the intelligent excavation robot, (δp _D ) ^T is the position error during dead reckoning, (δp _GL ) ^T is the lever arm error, (ε ^b ) ^T is the gyroscope bias error,

is the zero bias error of the accelerometer,

l is the position vector of strapdown inertial navigation and oil cylinder; The state equation of the combined measurement system is:

in,

and

are the white noise of gyro angular velocity measurement and the white noise of accelerometer specific force measurement; According to the error model in S201, S202, S203, the parameters in the state transition matrix F are as follows:

S302. Establish the system measurement equation of strapdown inertial navigation and oil cylinder The observation vector is constructed by the difference between the position calculated by the strapdown inertial navigation and the position calculated by the dead reckoning, namely Z=δp-δp _D Then 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 the measurement noise; S303, establish the strapdown inertial navigation and the standard Kalman filter equation of the oil cylinder to obtain the optimal estimated value of the system state, and the iterative process can be divided into the following five steps 1) One-step prediction of the state at the current moment through the state equation and the state of the system at the previous moment X(k/k-1)=F·X(k-1) Among them, X(k-1) is the state quantity of the system at time k-1; 2) Solve the mean square error matrix of the error of the predicted state at the current moment P(k/k-1)=FP(k-1)F ^T +Q P(k-1) is the mean square error matrix of the system state error at time k-1, and Q is the system noise matrix; 3) Solve the Kalman gain K K _k =P(k/k-1)H ^T (HP(k/k-1)H ^T +R) ^-1 where R is the measurement noise matrix; 4) Combine the Kalman gain K to update the optimal estimated value of the state at the current moment X(k)=X(k/k-1)+ _Kk ( _Zk -HX(k/k-1)) 5) Update the mean square error matrix of the optimal estimated value error of the state at the current moment P(k)=(IK _k H)P(k/k-1) S4. According to the fusion result of the forward displacement data of the cylinder-moving intelligent excavation robot and the strapdown inertial navigation data, the pose curve of the intelligent excavation robot is obtained, so as to realize the precise positioning and orientation of the excavation face. 3. a kind of intelligent excavation robot positioning and orientation method based on strapdown inertial navigation and oil cylinder as claimed in claim 1, it is characterized in that: when intelligent excavation robot goes straight, the oil cylinders on the left and right sides of the intelligent excavation robot are equal in displacement, and at this time The displacement of the intelligent excavation robot is equal to the displacement of the oil cylinder; when the intelligent excavation robot rectifies the deviation, the displacement of the oil cylinders on the left and right sides is not equal. Equivalent, and the displacement of the strapdown inertial navigation can be approximately equivalent to its projected displacement behind the robot I shield. At this time, the displacement data of the intelligent excavation robot displacement is as follows:

In the formula, a is the distance between the SINS projection and the left surface of the robot I shield, b is the distance between the SINS projection and the right surface of the robot I shield, L _l is the displacement of the left oil cylinder of the intelligent excavation robot, L _r is the displacement of the right oil cylinder of the intelligent excavation robot. 4. a kind of intelligent excavation robot positioning and orientation method based on strapdown inertial navigation and oil cylinder as claimed in claim 1, it is characterized in that: the intelligent excavation robot attitude data obtained by the strapdown inertial navigation solution and the oil cylinder moving intelligent excavation robot The forward displacement data is combined to form a dead reckoning system, and an error model of the dead reckoning system is constructed. 5. The method for positioning and orienting an intelligent excavation robot based on strapdown inertial navigation and an oil cylinder as claimed in claim 1, characterized in that: considering the rod arm error caused by the difference between the position of the oil cylinder and the installation position of the strapdown inertial navigation, and constructing Lever arm error model. 6. a kind of intelligent roadheading robot positioning and orientation method based on strapdown inertial navigation and oil cylinder as claimed in claim 1, is characterized in that: comprehensively consider the error of strapdown inertial navigation system, the error of dead reckoning system and lever arm error , using the standard Kalman filter to realize the fusion of the strapdown inertial navigation data and the oil cylinder data, and obtain the high-precision positioning and orientation data of the intelligent excavation robot.

Images