CN110411443B - Coal mining machine rocker arm inertia/vision combined attitude determination device and method - Google Patents
Coal mining machine rocker arm inertia/vision combined attitude determination device and method Download PDFInfo
- Publication number
- CN110411443B CN110411443B CN201910486547.XA CN201910486547A CN110411443B CN 110411443 B CN110411443 B CN 110411443B CN 201910486547 A CN201910486547 A CN 201910486547A CN 110411443 B CN110411443 B CN 110411443B
- Authority
- CN
- China
- Prior art keywords
- coordinate system
- axis
- mining machine
- coal mining
- rocker arm
- 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
Links
- 239000003245 coal Substances 0.000 title claims abstract description 102
- 238000005065 mining Methods 0.000 title claims abstract description 75
- 238000000034 method Methods 0.000 title claims abstract description 35
- 238000005259 measurement Methods 0.000 claims abstract description 90
- 230000010365 information processing Effects 0.000 claims abstract description 34
- 239000011159 matrix material Substances 0.000 claims description 32
- 230000001133 acceleration Effects 0.000 claims description 26
- 239000013598 vector Substances 0.000 claims description 18
- 230000003287 optical effect Effects 0.000 claims description 14
- 230000000007 visual effect Effects 0.000 claims description 14
- 238000004364 calculation method Methods 0.000 claims description 13
- NCGICGYLBXGBGN-UHFFFAOYSA-N 3-morpholin-4-yl-1-oxa-3-azonia-2-azanidacyclopent-3-en-5-imine;hydrochloride Chemical compound Cl.[N-]1OC(=N)C=[N+]1N1CCOCC1 NCGICGYLBXGBGN-UHFFFAOYSA-N 0.000 claims description 9
- 238000006243 chemical reaction Methods 0.000 claims description 6
- 238000003384 imaging method Methods 0.000 claims description 3
- 238000005096 rolling process Methods 0.000 claims description 3
- 230000007704 transition Effects 0.000 claims description 3
- 230000004927 fusion Effects 0.000 claims description 2
- 230000006978 adaptation Effects 0.000 abstract 1
- 238000000691 measurement method Methods 0.000 description 7
- 238000006073 displacement reaction Methods 0.000 description 4
- 238000005516 engineering process Methods 0.000 description 2
- 238000012544 monitoring process Methods 0.000 description 2
- 238000004804 winding Methods 0.000 description 2
- 238000005299 abrasion Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
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/18—Stabilised platforms, e.g. by gyroscope
Abstract
The device comprises a target characteristic point unit, wherein the target characteristic point unit is arranged on a rocker arm of the coal mining machine, an inertia measurement unit and a monocular vision measurement unit are rigidly connected and arranged on the coal mining machine, and an information processing unit is arranged in an electric control box of the coal mining machine; the monocular vision measuring unit is used for measuring the relative posture of the rocker arm relative to the coal mining machine, and the inertia measuring unit is used for measuring the absolute posture of the coal mining machine relative to a local horizontal coordinate system. The method is used for solving the problem of high-precision attitude determination of the rocker arm of the coal mining machine by simultaneously solving the attitude of the rocker arm relative to the coal mining machine and the attitude of the rocker arm relative to a local horizontal geographic coordinate system. The system has the advantages of simple structure, easy calibration and calibration of the camera, adaptation to the strong vibration environment of the rocker arm, complete attitude information and high precision, and is particularly suitable for the close-range high-precision pose measurement of the fully mechanized mining face.
Description
Technical Field
The invention relates to the technical field of coal cutter rocker arm attitude determination, in particular to a coal cutter rocker arm inertia/vision combined attitude determination device and method.
Background
In the automatic coal mining process, the coal mining machine is main equipment for coal falling and coal loading of a fully-mechanized coal face, and is high-integration fully-mechanized coal mining equipment. The rocker arm of the coal mining machine is an important component part of the coal mining machine, the posture of the rocker arm of the coal mining machine is a key parameter of the automation, remote control and on-line monitoring of the coal mining machine, and the automatic measurement and automatic control of the coal cutting track of the coal mining machine can be realized by measuring the posture of the rocker arm. The posture determination of the rocker arm of the coal mining machine is the basis of automatic control of the coal mining machine and is one of key technologies of automatic control of the coal mining machine and automatic fully-mechanized mining face.
The gesture determining technology is to obtain gesture information of a detected target through one or more sensors, and obtain the gesture angle of the detected target through direct observation or indirect observation by a resolving algorithm. The working condition of the working face of the coal mine is complex, and the space is closed, so that the problem of the attitude determination of the rocker arm of the coal mining machine is an indoor attitude determination problem under a typical complex environment.
The existing method for determining the pose of the rocker arm of the coal mining machine mainly comprises an angle displacement sensor method, a linear displacement sensor method, an inclination angle measurement method, a vision measurement method and the like.
The methods such as an angle displacement sensor method, a linear displacement measurement method and an inclination angle measurement method need to install a sensor on a rocker arm, and the main defects are that the severe vibration easily causes serious equipment abrasion, the sensor failure occurrence rate is high and the equipment working reliability is reduced because the rocker arm vibrates severely in the coal mining process.
The vision measurement method can only be used for measuring the relative attitude, and the attitude information is incomplete when the vision measurement method is used alone, so that the absolute attitude required by automatic control is difficult to obtain, and the requirement of full-automatic control of the coal mining machine cannot be met.
Disclosure of Invention
In order to solve the technical problems, the invention aims to provide an inertial/visual combined attitude determination device and method for a rocker arm of a coal mining machine, which are used for realizing high-precision absolute attitude determination and relative attitude determination of the rocker arm of the coal mining machine by combining monocular visual measurement and inertial measurement.
In order to achieve the above purpose, the technical scheme adopted by the invention is as follows:
the utility model provides a coal-winning machine rocking arm inertia/vision combination gesture device, includes target feature point unit 1, inertial measurement unit 3, monocular vision measurement unit 2 and information processing unit 4;
the target characteristic point unit 1 is arranged on a rocker arm of the coal mining machine, the inertia measurement unit 3 and the monocular vision measurement unit 2 are rigidly connected and arranged on the coal mining machine, and the information processing unit 4 is arranged in an electric control box of the coal mining machine;
the monocular vision measuring unit 2 is used for measuring the relative posture of the rocker arm relative to the coal mining machine, and the inertia measuring unit 3 is used for measuring the absolute posture of the coal mining machine relative to a local horizontal coordinate system.
The method for determining the attitude of the rocker arm of the coal mining machine by combining inertia and vision comprises the following steps of;
1) The target characteristic point unit 1 is arranged on the rocker arm and consists of 4 optical characteristic points, and the 4 characteristic points are distributed in a coplanar manner and are square;
2) The monocular vision measuring unit 2 shoots an image of a target feature point unit by using a single camera, solves a posture relation between a feature coordinate system and a monocular vision measuring unit body coordinate system by using a relative position constraint relation among a plurality of feature points on the image, and transmits the solved posture to the information processing unit 4;
3) The inertial measurement unit 3 is a device that measures the three-axis angular velocity and the three-axis acceleration of an object. In general, the inertial measurement unit 3 includes three single-axis accelerometers and three single-axis gyroscopes, the accelerometers measure acceleration signals of the object in three axial directions of the inertial measurement unit body coordinate system, the gyroscopes measure angular velocity signals of the inertial measurement unit 3 body coordinate system relative to the local horizontal geographic coordinate system, and the angular velocity and acceleration signals are transmitted to the information processing unit 4;
4) The information processing unit 4 receives the carrier three-axis angular velocity and three-axis acceleration signals, and utilizes a strapdown inertial navigation algorithm to calculate the attitude of the inertial measurement unit relative to a local horizontal geographic coordinate system;
5) The information processing unit 4 determines the relative posture and the absolute posture of the rocker arm of the coal mining machine through information fusion according to the relative posture of the rocker arm of the coal mining machine measured by the monocular vision measuring unit 2 and the absolute posture of the coal mining machine measured by the inertial measuring unit relative to a local horizontal coordinate system.
The specific calculation method comprises the following steps:
6) Defining an image coordinate system: the lower left point of the image is the origin of an image coordinate system, the horizontal axis is the u axis, the vertical axis is the v axis, and the image coordinate system takes the pixel number as a unit;
7) Definition of Rocker arm coordinate System O t X t Y t Z t : origin of coordinate system O t Is fixedly connected with the geometric center of 4 target characteristic points to form 4 targetsThe plane of the feature point is YOZ, X t The axis is outwards perpendicular to the plane of the target characteristic point, Y t Vertical axis upwards, Z t Axis and X t Axis, Y t The axes form a right hand coordinate system;
8) Definition of monocular vision measurement Unit coordinate System O c X c Y c Z c : origin of coordinate system O c Is fixedly connected with the perspective projection center of the camera, and when the optical axis of the lens points to the target characteristic unit, X is defined c The axis is positively directed to the target feature point unit along the optical axis of the lens, Y c Vertical axis upwards, Z c Axis and X c Axis, Y c The axes form a right hand coordinate system;
9) Definition of inertial measurement Unit coordinate System O b X b Y b Z b : origin of coordinate system O b Fixedly connected to the center of the inertial measurement unit, X b Axis, Y b Axis and Z b The axes being respectively with X c Axis, Y c Axis and Z c The axes are parallel. The coordinate system of the inertial measurement unit body is the same as the coordinate system of the coal mining machine body in three axial directions;
10 Defining a local horizontal geographic coordinate system O n X n Y n Z n : the North Tiandong geographic coordinate system;
11 During the working process of the coal mining machine, the monocular vision measuring unit 2 shoots the image of the target characteristic point unit 1 by using a single camera, solves the posture relation between the characteristic coordinate system and the monocular vision measuring unit 2 body coordinate system by using the relative position constraint relation among a plurality of characteristic points on the image, and transmits the solved posture to the information processing unit 4.
The specific process is as follows:
the monocular vision measurement unit 2 acquires the image of the target feature point unit 1 with a single camera, and the pixel coordinates of the 4 feature points on the image coordinate system are (u i ,v i ) I=1, 2,3,4. According to the proportion relation of pinhole imaging, there are
Where f is the camera focal length and d is the side physical dimension of a single square pixel, (u) 0 ,v 0 ) Is the pixel coordinate of the intersection of the optical axis and the image coordinate system. f, d, (u) 0 ,v 0 ) Are all of known quantity, (u) i ,v i ) Is the actual measurement value, and therefore (k xi ,k yi ) Calculated according to formula (1).
Characteristic point P i The coordinates of i=1, 2,3,4 in the feature coordinate system are (X ti ,Y ti ,Z ti ) While at the same time measuring the coordinates (X ci ,Y ci ,Z ci ) Then the conversion relationship of the two coordinate systems is as follows:
wherein ,(Tx ,T y ,T z ) T Representing the origin O of a characteristic coordinate system t To the origin O of the monocular vision measurement unit body coordinate system c And (5) translating the vector.
A state transition matrix (also called a direction cosine matrix) from the characteristic coordinate system t to the monocular vision measurement unit body coordinate system c is represented. Target coordinate system t-winding Y t The heading angle psi is rotated around Z t Axis rotation pitch angle θ, and then X t The axis rotates by a rolling angle gamma and then coincides with a monocular vision measurement unit body coordinate system c, namely
Order the
Since 4 feature points are distributed in square shape, the following is recorded
Substituting formula (2) into formula (1),
substituting the formula (4) into the formula (5) to obtain
Definition r i ＝c i K, i= 1,2,4,5,7,8 is substituted into formula (6), andequation (6) is changed to a 8-element linear equation set to (k) xi ,k yi ) As a known quantity, the unknown number can be obtained by solving by a known linear equation solution
Based on->
Is an identity orthogonal matrix, satisfy->
K can be calculated. According to r i I= 1,2,4,5,7,8 and K, c can be calculated i ，i＝1,2,4,5,7,8。
According to
Is a unit orthogonal matrix, and has the following unit vector cross relation:
c 0 ＝c 4 c 8 -c 7 c 5
c 3 ＝c 7 c 2 -c 1 c 8
c 6 ＝c 1 c 5 -c 4 c 2
pitch angle theta c ＝sin -1 (c 1 )
Course angle
Roll angle
12 An inertial measurement unit 3, including a three-axis gyroscope and a three-axis accelerometer. Angular velocity vectors of three axes of three-axis gyro measurement inertial measurement unit body coordinate system
Three-axis accelerometer for measuring acceleration vectors of three axial directions of inertial measurement unit body coordinate system
And the measured angular velocity +.>
And acceleration f b To the information processing unit.
13 The information processing unit receives the angular velocity and acceleration information of the inertial measurement unit to carry out strapdown inertial navigation position and posture calculation. Carrying out attitude calculation of the coal cutter by utilizing the triaxial angle of the coal cutter, updating an attitude quaternion and a direction cosine matrix, and extracting the attitude of the coal cutter relative to a local horizontal geographic coordinate system by utilizing the current position of the local horizontal geographic coordinate system; and projecting triaxial acceleration information of the coal cutter to three axial directions of a local horizontal geographic coordinate system by using a posture direction cosine matrix, obtaining axial acceleration of the coal cutter along the local horizontal geographic coordinate system, integrating the acceleration, and determining the three-dimensional speed and the three-dimensional position of the coal cutter on the local horizontal geographic coordinate system. The method comprises the following steps:
initial k=0, the information processing unit receives external binding information: longitude lambda (0), latitude
Height h (0) and initial posture +.>
At the same time, the initial velocity v= [0 0 0 0] T 。
At time k, the information processing unit first updates the angular velocity vector of the inertial measurement unit body coordinate system relative to the local horizontal geographic coordinate system
wherein ,
representing latitude values>
Is a gesture matrix omega ie Is the rotation angular velocity of the earth, R is the radius of the earth, V E and VN Indicating the east and north speeds.
Then, the quaternion q= [ q ] is updated 0 q 1 q 2 q 3 ] T
wherein ,
Δt represents the attitude calculation period.
Then, the inertial attitude matrix of the inertial measurement unit 3 with respect to the local horizontal geographic coordinate system is updated
Order the
Wherein i, j=1, 2,3
According to
The attitude angle of the shearer coordinate system relative to the local horizontal geographic coordinate system is calculated as follows:
pitch angle theta b ＝sin -1 (C 12 )
Course angle
Roll angle
By means of
Acceleration vector of accelerometer output +.>
Projected to a local horizontal geographic coordinate system f n ＝[f N f U f E ] T ，/>
Then, the velocity v= [ V ] of the inertial measurement unit 3 is updated N V U V E ] T Longitude, lambda, latitude
And a height h. />
h(k+1)＝h(k)+0.5ΔT(V U (k)+V U (k+1))
Let k +1, the calculation of the next moment is continued.
14 Relative attitude matrix of rocker arm coordinate system received by information processing unit relative to coal cutter body coordinate system
Simultaneously using the attitude matrix of the inertial measurement unit coordinate system calculated in step 4) relative to the local horizontal geographical coordinate system ∈>
Performing coordinate conversion to calculate an absolute posture matrix of the rocker arm coordinate system relative to a local horizontal geographic coordinate system>
15 Instruction) command
Then the absolute attitude angle is extracted:
pitch angle θ=sin -1 (C 12 )
Course angle
Roll angle
16 Output attitude angle includes:
attitude angle of the rocker arm relative to the shearer: pitch angle theta c Heading angle psi c And a roll angle gamma c
Absolute attitude angle of rocker arm relative to local geographic horizontal coordinate system: pitch angle θ, heading angle ψ, and roll angle γ.
Absolute attitude angle of the shearer relative to the local geographic horizontal coordinate system: pitch angle theta b Heading angle psi b And a roll angle gamma b 。
The invention has the beneficial effects that:
the invention provides an inertia/vision combined attitude determination device and method for a rocker arm of a coal mining machine, which are used for measuring the high-precision relative attitude of the rocker arm of the coal mining machine relative to the coal mining machine and the high-precision absolute attitude of the rocker arm of the coal mining machine relative to a local horizontal geographic coordinate system, and are used for automatically monitoring the absolute height of a cutting roller in real time, facilitating the automatic height adjustment of the cutting roller, enabling the roller to adapt to the fluctuation change of a coal bed, effectively preventing the cutting roller from cutting a roof, facilitating the automatic acquisition of the height of the coal bed cut by the coal mining machine, facilitating the automatic measurement of the track of the coal bed cut, the automatic tracking of the coal bed change and the like.
Drawings
Fig. 1 shows a combined inertial/visual attitude determination device for a rocker arm of a coal mining machine (a left rocker arm is taken as an example).
Fig. 2 is an image coordinate system, a feature coordinate system, a monocular vision measurement unit body coordinate system, and an inertial measurement unit body coordinate system example 1.
FIG. 3 is a schematic block diagram of a combined inertial/visual attitude determination method for a rocker arm of a coal mining machine.
Fig. 4 is a target feature point unit example 1.
In the figure: 1. a target feature point unit; 2. a monocular vision measurement unit; 3. an inertial measurement unit; 4 an information processing unit; 5. optical characteristic points
Detailed Description
The invention is described in further detail below with reference to the accompanying drawings.
Example 1: as shown in fig. 1, the rocker arm inertia/vision combined attitude determination device of the coal mining machine comprises a target characteristic point unit 1, a monocular vision measurement unit 2, an inertia measurement unit 3 and an information processing unit 4. The target characteristic point unit 1 is arranged on a rocker arm of the coal mining machine and consists of 4 optical characteristic points, wherein the 4 characteristic points are distributed in a coplanar manner and are square, as shown in fig. 4; the inertial measurement unit 3 is rigidly connected with the monocular vision measurement unit 2 and is arranged on the coal cutter body, so that the optical characteristic points are ensured to be all located in the view field range; the information processing unit 4 is arranged in the electric control box of the coal mining machine, and completes information acquisition and processing of the monocular vision measuring unit 2 and the inertia measuring unit 3.
As shown in fig. 3, the inertial/visual combined attitude determination method of the rocker arm of the coal mining machine comprises the following steps:
1) Defining an image coordinate system: the lower left point of the image is the origin of the image coordinate system, the horizontal axis is the u axis, the vertical axis is the v axis, and the image coordinate system is in units of pixels.
2) Definition of Rocker arm coordinate System O t X t Y t Z t : origin of coordinate system O t Is fixedly connected with the geometric center of 4 target feature points, and takes the plane where the 4 target feature points are positioned as YOZ and X t The axis is outwards perpendicular to the plane of the target characteristic point, Y t Vertical axis upwards, Z t Axis and X t Axis, Y t The axes form the right hand coordinate system as shown in fig. 2.
3) Definition of monocular vision measurement Unit coordinate System O c X c Y c Z c : origin of coordinate system O c Is fixedly connected with the perspective projection center of the camera, and when the optical axis of the lens points to the target characteristic unit, X is defined c The axis is positively directed to the target feature point unit along the optical axis of the lens, Y c Vertical axis upwards, Z c Axis and X c Axis, Y c The axes form the right hand coordinate system as shown in fig. 2.
4) Definition of inertial measurement UnitBody coordinate system O b X b Y b Z b : origin of coordinate system O b Fixedly connected to the center of the inertial measurement unit, X b Axis, Y b Axis and Z b The axes being respectively with X c Axis, Y c Axis and Z c The axes are parallel. The coordinate system of the inertial measurement unit body is the same as the coordinate system of the coal mining machine body in three axial directions. As shown in fig. 2.
5) Defining a local horizontal geographic coordinate System O n X n Y n Z n : the north eastern geographic coordinate system.
6) In the working process of the coal mining machine, the monocular vision measuring unit shoots an image of a target feature point unit by using a single camera, solves the posture relation between a feature coordinate system and a monocular vision measuring unit body coordinate system by using the relative position constraint relation among a plurality of feature points on the image, and transmits the solved posture (hereinafter referred to as a vision posture) to the information processing unit. The specific process is as follows:
the monocular vision measurement unit 2 acquires the image of the target feature point unit 1 with a single camera, and the pixel coordinates of the 4 feature points on the image coordinate system are (u i ,v i ) I=1, 2,3,4. According to the proportion relation of pinhole imaging, there are
Where f is the camera focal length and d is the side physical dimension of a single square pixel, (u) 0 ,v 0 ) Is the pixel coordinate of the intersection of the optical axis and the image coordinate system. f, d, (u) 0 ,v 0 ) Are all of known quantity, (u) i ,v i ) Is the actual measurement value, and therefore (k xi ,k yi ) Calculated according to formula (1).
Characteristic point P i The coordinates of i=1, 2,3,4 in the feature coordinate system are (X ti ,Y ti ,Z ti ) While at the same time measuring the coordinates (X ci ,Y ci ,Z ci ) Then the conversion relationship of the two coordinate systems is as follows:
wherein ,(Tx ,T y ,T z ) T Representing the origin O of a characteristic coordinate system t To the origin O of the monocular vision measurement unit body coordinate system c And (5) translating the vector.
A state transition matrix (also called a direction cosine matrix) from the characteristic coordinate system t to the monocular vision measurement unit body coordinate system c is represented. Target coordinate system t-winding Y t The heading angle psi is rotated around Z t Axis rotation pitch angle θ, and then X t The axis rotates by a rolling angle gamma and then coincides with a monocular vision measurement unit body coordinate system c, namely
Order the
Since 4 feature points are distributed in square shape, the following is recorded
Substituting formula (2) into formula (1),
substituting the formula (4) into the formula (5) to obtain
Definition r i ＝c i K, i= 1,2,4,5,7,8 is substituted into equation (6), and equation (6) is changed to 8-element linear equation set to (K) xi ,k yi ) As a known quantity, the unknown number can be obtained by solving by a known linear equation solution
Based on->
Is an identity orthogonal matrix, satisfy->
K can be calculated. According to r i I= 1,2,4,5,7,8 and K, c can be calculated i ，i＝1,2,4,5,7,8。
According to
Is a unit orthogonal matrix, and has the following unit vector cross relation:
c 0 ＝c 4 c 8 -c 7 c 5
c 3 ＝c 7 c 2 -c 1 c 8
c 6 ＝c 1 c 5 -c 4 c 2
pitch angle theta c ＝sin -1 (c 1 )
Course angle
Roll angle
7) The inertial measurement unit 3 includes a three-axis gyro and a three-axis accelerometer. Angular velocity vectors of three axes of three-axis gyro measurement inertial measurement unit body coordinate system
Three-axis accelerometer for measuring acceleration vectors of three axial directions of inertial measurement unit body coordinate system
And the measured angular velocity +.>
And acceleration f b To the information processing unit.
8) And the information processing unit receives the angular velocity and acceleration information of the inertial measurement unit to carry out strapdown inertial navigation position and posture calculation. Carrying out attitude calculation of the coal cutter by utilizing the triaxial angle of the coal cutter, updating an attitude quaternion and a direction cosine matrix, and extracting the attitude of the coal cutter relative to a local horizontal geographic coordinate system by utilizing the current position of the local horizontal geographic coordinate system; and projecting triaxial acceleration information of the coal cutter to three axial directions of a local horizontal geographic coordinate system by using a posture direction cosine matrix, obtaining axial acceleration of the coal cutter along the local horizontal geographic coordinate system, integrating the acceleration, and determining the three-dimensional speed and the three-dimensional position of the coal cutter on the local horizontal geographic coordinate system. The method comprises the following steps:
initial k=0, the information processing unit receives external binding information: longitude lambda (0), latitude
Height h (0) and initial posture +.>
At the same time, the initial velocity v= [0 0 0 0] T 。
At time k, the information processing unit first updates the angular velocity vector of the inertial measurement unit body coordinate system relative to the local horizontal geographic coordinate system
wherein ,
representing latitude values>
Is a gesture matrix omega ie Is the rotation angular velocity of the earth, R is the radius of the earth, V E and VN Indicating the east and north speeds.
Then, the quaternion q= [ q ] is updated 0 q 1 q 2 q 3 ] T
wherein ,
Δt represents the attitude calculation period.
Then, the inertial attitude matrix of the inertial measurement unit 3 with respect to the local horizontal geographic coordinate system is updated
Order the
Wherein i, j=1, 2,3
According to
The attitude angle of the shearer coordinate system relative to the local horizontal geographic coordinate system is calculated as follows:
pitch angle theta b ＝sin -1 (C 12 )
Course angle
/>
Roll angle
By means of
Acceleration vector of accelerometer output +.>
Projected to a local horizontal geographic coordinate system f n ＝[f N f U f E ] T ，/>
Then, the velocity v= [ V ] of the inertial measurement unit 3 is updated N V U V E ] T Longitude, lambda, latitude
And a height h.
h(k+1)＝h(k)+0.5ΔT(V U (k)+V U (k+1))
Let k +1, the calculation of the next moment is continued.
9) Relative attitude matrix of rocker arm coordinate system received by information processing unit relative to coal cutter body coordinate system
Simultaneously using the attitude matrix of the inertial measurement unit coordinate system calculated in step 4) relative to the local horizontal geographical coordinate system ∈>
Performing coordinate conversion to calculate an absolute posture matrix of the rocker arm coordinate system relative to a local horizontal geographic coordinate system>
10 Instruction) command
Then the absolute attitude angle is extracted:
pitch angle θ=sin -1 (C 12 )
Course angle
Roll angle
11 Output attitude angle includes:
attitude angle of the rocker arm relative to the shearer: pitch angle theta c Heading angle psi c And a roll angle gamma c
Absolute attitude angle of rocker arm relative to local geographic horizontal coordinate system: pitch angle θ, heading angle ψ, and roll angle γ.
Absolute attitude angle of the shearer relative to the local geographic horizontal coordinate system: pitch angle theta b Heading angle psi b And a roll angle gamma b 。
According to the invention, unmanned mining on the underground working face is used as an application background, and the high-precision attitude determination of the rocker arm of the coal mining machine in the closed space is realized by utilizing an inertia/vision combination method. The inertial attitude measurement is a full-automatic attitude determination method and has the advantages of low delay and high speed. The vision measurement adopts a monocular vision measurement method, has the advantages of simple system structure, easy camera calibration and calibration, high precision and low cost, and is particularly suitable for indoor short-distance high-precision pose measurement.
The invention combines inertial measurement and visual measurement, provides a device and a method for determining the attitude of a rocker arm of a coal mining machine by combining inertial/visual measurement, and simultaneously solves the problems of high-precision attitude determination of the rocker arm of the coal mining machine by solving the attitude (relative attitude determination) of the rocker arm relative to the coal mining machine and the attitude (absolute attitude determination) of the rocker arm relative to a local horizontal geographic coordinate system.
Claims (5)
1. The method is characterized by being realized by a coal cutter rocker arm inertia/vision combined attitude determination device, and the device comprises a target characteristic point unit, an inertia measurement unit, a monocular vision measurement unit and an information processing unit;
the target characteristic point unit is arranged on a rocker arm of the coal mining machine, the inertia measurement unit and the monocular vision measurement unit are rigidly connected and arranged on the coal mining machine, and the information processing unit is arranged in an electric control box of the coal mining machine;
the monocular vision measuring unit is used for measuring the relative posture of the rocker arm relative to the coal mining machine, and the inertia measuring unit is used for measuring the absolute posture of the coal mining machine relative to a local horizontal coordinate system;
the method comprises the following steps;
1) The target characteristic point unit is arranged on the rocker arm and consists of 4 optical characteristic points, and the 4 characteristic points are distributed in a coplanar manner and are square;
2) The monocular vision measuring unit shoots an image of the target feature point unit by using a single camera, solves the posture relation between a feature coordinate system and a monocular vision measuring unit body coordinate system by using the relative position constraint relation among a plurality of feature points on the image, and transmits the solved posture to the information processing unit;
3) The inertial measurement unit is a device for measuring the three-axis angular rate and the three-axis acceleration of an object, and generally comprises three single-axis accelerometers and three single-axis gyroscopes, wherein the accelerometers measure acceleration signals of the object in three axial directions of a body coordinate system of the inertial measurement unit, the gyroscopes measure angular velocity signals of the body coordinate system of the inertial measurement unit (3) relative to a local horizontal geographic coordinate system, and the angular velocity signals and the acceleration signals are transmitted to the information processing unit;
4) The information processing unit receives the triaxial angular speed and triaxial acceleration signals of the carrier, and utilizes a strapdown inertial navigation algorithm to calculate the posture of the inertial measurement unit relative to a local horizontal geographic coordinate system;
5) The information processing unit determines the relative posture and the absolute posture of the rocker arm of the coal mining machine through information fusion according to the relative posture of the rocker arm of the coal mining machine measured by the monocular vision measuring unit and the absolute posture of the coal mining machine measured by the inertial measuring unit relative to a local horizontal coordinate system;
the information processing unit receives the angular velocity and acceleration information of the inertial measurement unit to carry out strapdown inertial navigation position and posture calculation; carrying out attitude calculation of the coal cutter by utilizing the triaxial angle of the coal cutter, updating an attitude quaternion and a direction cosine matrix, and extracting the attitude of the coal cutter relative to a local horizontal geographic coordinate system by utilizing the current position of the local horizontal geographic coordinate system; projecting triaxial acceleration information of the coal mining machine to three axial directions of a local horizontal geographic coordinate system by using a posture direction cosine matrix, obtaining axial acceleration of the coal mining machine along the local horizontal geographic coordinate system, integrating the acceleration, and determining three-dimensional speed and position of the coal mining machine on the local horizontal geographic coordinate system;
the method comprises the following steps:
initial k=0, the information processing unit receives external binding information: longitude lambda (0), latitude
Height h (0) and initial posture +.>
At the same time, the initial velocity v= [0 0 0 0] T ；
At time k, the information processing unit first updates the angular velocity vector of the inertial measurement unit body coordinate system relative to the local horizontal geographic coordinate system
wherein ,
representing latitude values>
Is a gesture matrix omega ie Is the rotation angular velocity of the earth, R is the radius of the earth, V E and VN Representing east and north speeds;
then, the quaternion q= [ q ] is updated 0 q 1 q 2 q 3 ] T
wherein ,
Δt represents a posture calculation period;
then, the inertial attitude matrix of the inertial measurement unit 3 with respect to the local horizontal geographic coordinate system is updated
Order the
Wherein i, j=1, 2,3
According to
The attitude angle of the shearer coordinate system relative to the local horizontal geographic coordinate system is calculated as follows:
pitch angle theta b ＝sin -1 (C 12 )
Course angle
Roll angle
By means of
Acceleration vector of accelerometer output +.>
Projected to a local horizontal geographic coordinate system f n ＝[f N f U f E ] T ，/>
Then, the velocity v= [ V ] of the inertial measurement unit 3 is updated N V U V E ] T Longitude, lambda, latitude
And a height h;
h(k+1)＝h(k)+0.5ΔT(V U (k)+V U (k+1))
let k +1, the calculation of the next moment is continued.
2. The method for determining the inertial/visual combined attitude of a rocker arm of a coal mining machine according to claim 1, wherein the method comprises the following steps of,
defining an image coordinate system: the lower left point of the image is the origin of an image coordinate system, the horizontal axis is the u axis, the vertical axis is the v axis, and the image coordinate system takes the pixel number as a unit;
definition of Rocker arm coordinate System O t X t Y t Z t : origin of coordinate system O t Is fixedly connected with the geometric center of 4 target feature points, and takes the plane where the 4 target feature points are positioned as YOZ and X t The axis is outwards perpendicular to the plane of the target characteristic point, Y t Vertical axis upwards, Z t Axis and X t Axis, Y t The axes form a right hand coordinate system;
definition of monocular vision measurement Unit coordinate System O c X c Y c Z c : origin of coordinate system O c Is fixedly connected with the perspective projection center of the camera, and when the optical axis of the lens points to the target characteristic unit, X is defined c The axial direction points to the target characteristic point along the optical axis of the lensUnit, Y c Vertical axis upwards, Z c Axis and X c Axis, Y c The axes form a right hand coordinate system;
definition of inertial measurement Unit coordinate System O b X b Y b Z b : origin of coordinate system O b Fixedly connected to the center of the inertial measurement unit, X b Axis, Y b Axis and Z b The axes being respectively with X c Axis, Y c Axis and Z c The axes are parallel; the coordinate system of the inertial measurement unit body is the same as the coordinate system of the coal mining machine body in three axial directions;
defining a local horizontal geographic coordinate System O n X n Y n Z n : the North Tiandong geographic coordinate system;
in the working process of the coal mining machine, the monocular vision measuring unit 2 shoots an image of the target characteristic point unit 1 by using a single camera, solves the posture relation between a characteristic coordinate system and a monocular vision measuring unit 2 body coordinate system by using the relative position constraint relation among a plurality of characteristic points on the image, and transmits the solved posture to the information processing unit 4.
3. The method for determining the inertial/visual combined attitude of a rocker arm of a coal mining machine according to claim 1, wherein the method comprises the following steps of,
the monocular vision measuring unit collects images of the target feature point unit by using a single camera, and pixel coordinates of the 4 feature points on an image coordinate system are (u) i ,v i ) I=1, 2,3,4; according to the proportion relation of pinhole imaging, there are
Where f is the camera focal length and d is the side physical dimension of a single square pixel, (u) 0 ,v 0 ) Pixel coordinates, f, d, (u) which are the intersection points of the optical axis and the image coordinate system 0 ,v 0 ) Are all of known quantity, (u) i ,v i ) Is the actual measurement value, and therefore (k xi ,k yi ) Calculated according to formula (1);
characteristic point P i The coordinates of i=1, 2,3,4 in the feature coordinate system are (X ti ,Y ti ,Z ti ) While at the same time measuring the coordinates (X ci ,Y ci ,Z ci ) Then the conversion relationship of the two coordinate systems is as follows:
wherein ,(Tx ,T y ,T z ) T Representing the origin O of a characteristic coordinate system t To the origin O of the monocular vision measurement unit body coordinate system c Translation vector;
representing a state transition matrix from a characteristic coordinate system t to a monocular vision measurement unit body coordinate system c, and a target coordinate system t is wound around Y t The heading angle psi is rotated around Z t Axis rotation pitch angle θ, and then X t The axis rotates by a rolling angle gamma and then coincides with a monocular vision measurement unit body coordinate system c, namely
Order the
Since 4 feature points are distributed in square shape, the following is recorded
Substituting formula (2) into formula (1),
substituting the formula (4) into the formula (5) to obtain
Definition r i ＝c i K, i= 1,2,4,5,7,8 is substituted into equation (6), and equation (6) is changed to 8-element linear equation set to (K) xi ,k yi ) As a known quantity, the unknown number can be obtained by solving by a known linear equation solution
Based on->
Is an identity orthogonal matrix, satisfy->
K can be calculated; according to r i I= 1,2,4,5,7,8 and K, c can be calculated i ，i＝1,2,4,5,7,8；
According to
Is a unit orthogonal matrix, and has the following unit vector cross relation:
c 0 ＝c 4 c 8 -c 7 c 5
c 3 ＝c 7 c 2 -c 1 c 8
c 6 ＝c 1 c 5 -c 4 c 2
pitch angle theta c ＝sin -1 (c 1 )
Course angle
Roll angle
4. The method for determining the inertial/visual combined attitude of the rocker arm of the coal mining machine according to claim 1, wherein the inertial measurement unit (3) comprises a three-axis gyroscope and a three-axis accelerometer, and the three-axis gyroscope is used for measuring three coordinate systems of the inertial measurement unit bodyAngular velocity vector of individual axes
Three-axis accelerometer for measuring acceleration vectors of three axial directions of inertial measurement unit body coordinate system
And the measured angular velocity +.>
And acceleration f b To the information processing unit.
5. The combined inertial/visual pose determination method of rocker arm of coal mining machine according to claim 1, wherein said information processing unit receives relative pose matrix of rocker arm coordinate system relative to coal mining machine body coordinate system
Simultaneously using the attitude matrix of the inertial measurement unit coordinate system calculated in step 4) relative to the local horizontal geographical coordinate system ∈>
Performing coordinate conversion to calculate an absolute posture matrix of the rocker arm coordinate system relative to a local horizontal geographic coordinate system>
Order the
Then the absolute attitude angle is extracted:
pitch angle θ=sin -1 (C 12 )
Course angle
/>
Roll angle
The output attitude angle includes:
attitude angle of the rocker arm relative to the shearer: pitch angle theta c Heading angle psi c And a roll angle gamma c ；
Absolute attitude angle of rocker arm relative to local geographic horizontal coordinate system: pitch angle θ, heading angle ψ, and roll angle γ;
absolute attitude angle of the shearer relative to the local geographic horizontal coordinate system: pitch angle theta b Heading angle psi b And a roll angle gamma b 。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910486547.XA CN110411443B (en) | 2019-06-05 | 2019-06-05 | Coal mining machine rocker arm inertia/vision combined attitude determination device and method |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910486547.XA CN110411443B (en) | 2019-06-05 | 2019-06-05 | Coal mining machine rocker arm inertia/vision combined attitude determination device and method |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN110411443A CN110411443A (en) | 2019-11-05 |
| CN110411443B true CN110411443B (en) | 2023-05-02 |
Family
ID=68358882
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201910486547.XA Active CN110411443B (en) | 2019-06-05 | 2019-06-05 | Coal mining machine rocker arm inertia/vision combined attitude determination device and method |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN110411443B (en) |
Families Citing this family (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111207742B (en) * | 2020-01-17 | 2020-12-15 | 西安科技大学 | Coal mining machine positioning and attitude determining method with additional external orientation element constraint |
| CN111735445B (en) * | 2020-06-23 | 2022-02-11 | 煤炭科学研究总院 | Monocular vision and IMU (inertial measurement Unit) integrated coal mine tunnel inspection robot system and navigation method |
| CN112814676A (en) * | 2020-12-31 | 2021-05-18 | 重庆大学 | Coal cutting track dynamic correction method based on fully mechanized coal mining face coal seam three-dimensional model construction |
Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105698765A (en) * | 2016-02-22 | 2016-06-22 | 天津大学 | Method using combination of double IMUs (inertial measurement units) and monocular vision to measure pose of target object under non-inertial system |
| CN108731672A (en) * | 2018-05-30 | 2018-11-02 | 中国矿业大学 | Coalcutter attitude detection system and method based on binocular vision and inertial navigation |
| CN109443206A (en) * | 2018-11-09 | 2019-03-08 | 山东大学 | Mechanical arm tail end pose measurement system and method based on color spherical fishing light source target |
| CN109579831A (en) * | 2018-11-09 | 2019-04-05 | 西安科技大学 | Mining boom-type roadheader visualization auxiliary guidance method and system |
Family Cites Families (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| FR2998363B1 (en) * | 2012-11-19 | 2015-01-02 | Inria Inst Nat Rech Informatique & Automatique | METHOD FOR DETERMINING, IN A 3D FIXED REFERENTIAL, THE LOCATION OF A MOVING GEAR, ASSOCIATED DEVICE AND COMPUTER PROGRAM |
| US9946264B2 (en) * | 2016-03-22 | 2018-04-17 | Sharp Laboratories Of America, Inc. | Autonomous navigation using visual odometry |
-
2019
- 2019-06-05 CN CN201910486547.XA patent/CN110411443B/en active Active
Patent Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105698765A (en) * | 2016-02-22 | 2016-06-22 | 天津大学 | Method using combination of double IMUs (inertial measurement units) and monocular vision to measure pose of target object under non-inertial system |
| CN108731672A (en) * | 2018-05-30 | 2018-11-02 | 中国矿业大学 | Coalcutter attitude detection system and method based on binocular vision and inertial navigation |
| CN109443206A (en) * | 2018-11-09 | 2019-03-08 | 山东大学 | Mechanical arm tail end pose measurement system and method based on color spherical fishing light source target |
| CN109579831A (en) * | 2018-11-09 | 2019-04-05 | 西安科技大学 | Mining boom-type roadheader visualization auxiliary guidance method and system |
Non-Patent Citations (2)
| Title |
|---|
| 基于辅助匹配的1点RANSAC单目视觉导航算法;齐乃新;《系统工程与电子技术》;20180531;第40卷(第05期);第1109-1117页 * |
| 立体视觉惯性信息紧耦合自主机器人定位研究;郭永祥;《中国优秀硕士学位论文全文数据库信息科技辑》;20180615(第06期);第I138-1340页 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN110411443A (en) | 2019-11-05 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN1788188B (en) | Picked-up image display method and device | |
| EP2772725B1 (en) | Aerial Photographing System | |
| CN109911188B (en) | Bridge detection unmanned aerial vehicle system in non-satellite navigation and positioning environment | |
| CN110411443B (en) | Coal mining machine rocker arm inertia/vision combined attitude determination device and method | |
| Li et al. | LIDAR/MEMS IMU integrated navigation (SLAM) method for a small UAV in indoor environments | |
| CN110132309B (en) | Calibration method of rocker arm inertia/vision combined attitude determination device of coal mining machine | |
| CN103900611B (en) | Method for aligning two composite positions with high accuracy and calibrating error of inertial navigation astronomy | |
| CN108235735A (en) | Positioning method and device, electronic equipment and computer program product | |
| CN106814753B (en) | Target position correction method, device and system | |
| CN110702109B (en) | Coal mining machine inertial navigation/wireless sensor network combined positioning method | |
| JP2008186145A (en) | Aerial image processing apparatus and aerial image processing method | |
| US10800344B2 (en) | Aerial photogrammetric device and aerial photogrammetric method | |
| ES2655978T3 (en) | Procedure for estimating the angular deviation of a moving element in relation to a reference direction | |
| CN105160125B (en) | A kind of simulating analysis of star sensor quaternary number | |
| KR101943823B1 (en) | UAV for accurate position data acquisition during high-speed flight and Determination of Unmanned Mission Equipment Synchronization for Accurate Position Data Acquisition during High Speed Flight | |
| CN110095659B (en) | Dynamic testing method for pointing accuracy of communication antenna of deep space exploration patrol device | |
| CN108444468B (en) | Directional compass integrating downward vision and inertial navigation information | |
| CN111247389B (en) | Data processing method and device for shooting equipment and image processing equipment | |
| Andert et al. | On the safe navigation problem for unmanned aircraft: Visual odometry and alignment optimizations for UAV positioning | |
| CN109839109B (en) | Development machine absolute pose detection method based on image recognition and multi-sensor fusion | |
| CN102288175B (en) | Independent forecasting method suitable for star sensor | |
| CN112857367B (en) | Heading machine pose detection method based on machine vision and inertial navigation | |
| KR20100064606A (en) | System and method for manufacturing numerical map using gps and ins | |
| Vintervold | Camera-based integrated indoor positioning | |
| CN216577944U (en) | Camera robot virtual-real fusion precision optimization device based on optical inertial tracking |
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 |