Note: Descriptions are shown in the official language in which they were submitted.
CA 03071299 2020-01-28
INITIAL ALIGNMENT SYSTEM AND METHOD FOR STRAP-DOWN INERTIAL
NAVIGATION OF SHEARER BASED ON OPTICAL FLOW METHOD
TECHNICAL FIELD
The present invention relates to an initial alignment system and method for a
shearer,
and in particular, to an initial alignment system and method for strap-down
inertial navigation
of a shearer based on an optical flow method.
BACKGROUND OF THE INVENTION
Coal is the most widely distributed and most abundant energy resource in the
world and
has always dominated the world's energy system. Coal is the basic energy and
raw material of
national economy in China, and accounts for about 70% of primary energy
sources. Although
China calls for energy conservation and emission reduction and encourages the
development
of new energy sources in recent years, coal-based energy structure plays an
important role in
national economic production activities. Therefore, whether the coal industry
can be
developed healthily and stably is of great significance for energy stability
and economic
development in China.
In order to realize the linkage of "three machines" for mining, it is of great
significance
to accurately detect the spatial position and attitude of a shearer, namely,
to dynamically
position the shearer. In order to realize the position and attitude detection
of the shearer, some
scholars have proposed the inertial navigation positioning method of the
shearer. A
strap-down inertial navigation system refers to directly fixing a gyroscope
and an
accelerometer on a carrier, measuring triaxial angular velocity and triaxial
acceleration
information of the running carrier in real time by using inertia sensitive
components such as
the gyroscope and the accelerometer, and obtaining navigation information such
as attitude,
velocity and position of the running carrier through high-velocity integration
in combination
with the initial inertia information of the running carrier. During operation,
the strap-down
inertial navigation system does not rely on external information, does not
radiate energy to
the outside, and is not susceptible to interference and damage. It is an
autonomous navigation
system with the advantages of high data update rate, comprehensive data and
high short-term
positioning accuracy. The method uses external velocity assistance without the
need for a
coarse alignment stage to achieve accurate initial alignment of a movable base
of the
strap-down inertial navigation.
1
CA 03071299 2020-01-28
Prior to operation, the inertial navigation system first initializes the
navigation
information, wherein the process of obtaining the initial attitude information
is called initial
alignment. However, because the shearer is easily interfered during operation,
resulting in
shaking of the body of the shearerõ so that the original detection of the
rotation angular
velocity of the earth by the gyroscope is easily masked by the angular
velocity of motion of
the bodyõ the conventional analytical method has a too large initial alignment
error and even
is unusable, and the initial alignment based on the inertial coordinate system
has better
anti-interference ability for angular shaking.
The algorithm of the initial alignment based on the inertial coordinate system
requires
the velocity over ground of the shearer. The conventional video velocity
measurement
algorithms include a background subtraction method, a frame subtraction
method, and an
optical flow method, etc. The background difference method cannot adapt to the
scene
change well. The frame subtraction method cannot completely extract the state
of all relevant
feature points, so that the obtained image is not a pure background image,
resulting in
inaccurate detection results, which is not conducive to target analysis and
velocity detection.
SUMMARY OF THE INVENTION
Regarding the foregoing problems in the prior art, the present invention
provides an
initial alignment system and method for strap-down inertial navigation of a
shearer based on
an optical flow method, which improves the error correction for accurate
initial alignment of
a movable base of the shearer without a coarse alignment stage, to achieve
accurate initial
alignment of the movable base of the strap-down inertial navigation.
To achieve the foregoing objective, the technical solution adopted by the
present
invention is: an initial alignment system for strap-down inertial navigation
of a shearer based
on an optical flow method, comprising an explosion-proof box, a strap-down
inertial
navigation system, a processor, a fixed support, and a camera, wherein the
explosion-proof
box is fixedly mounted on the body of the shearer; the strap-down inertial
navigation system
and the processor are mounted in the explosion-proof box; and the camera is
fixed on a
hydraulic support at one side of the shearer by means of the fixed support,
with the
photographing direction of the camera facing toward the shearer.
Further, the processor comprises a micro-processing unit module, a
communication
module, an alarm module, a data storage module, an isolation circuit, and a
power module.
2
The micro-processing unit module is connected to the communication module, the
alarm
module, the data storage module, the isolation circuit, and the power module,
respectively.
Further, the micro-processing unit module in the processor selects a DSP chip,
such as
one available from Texas Instruments Incorporated.
Further, the explosion-proof box is a special explosion-proof box for coal
mines.
Further, the camera is hingedly connected to the fixed support.
Further, the strap-down inertial navigation system adopts a laser strap-down
inertial
navigation system, wherein the random drift stability of a laser gyroscope is
0.01 /h, and the
bias stability of an accelerometer is 10-5 g.
An initial alignment method for strap-down inertial navigation of shearer
based on
optical flow method comprises the following specific steps:
A. photographing, by a camera with the camera frame rate of 25 frames/s, an
image of
the environment where a shearer is located, and transmitting the photographed
image to a
processor;
B. performing, by the processor, gray-scale processing on the photographed
image in an
image gray-scale mode; when the shearer moves in the photographing
environment, so that
the photographed target image changes, and the surface motion of the image
gray-scale mode
is optical flow, determining the moving direction of the shearer based on the
principle of
principal direction of motion according to the relationship between a motion
field and an
optical flow field of the shearer;
C. calculating the optical flow velocity of the shearer moving in the image by
using the
Lucas-Kanade optical flow method, and converting the calculated optical flow
velocity in the
image into the actual velocity over ground of the shearer, marked 1, 11, to
obtain velocity
information in the motion direction of the shearer;
E. projecting specific force information onto an inertial coordinate system by
using a
specific force equation of strap-down inertial navigation, to obtain direction
change
information of the specific force relative to the inertial space as the earth
rotates, with the
specific force equation being:
3
Date Recue/Date Received 2021-08-05
CA 03071299 2020-01-28
(t) (0+ a. (t))x vh (1) - fib (t) = gb
wherein at(t) is the angular velocity of a body coordinate system, 01,',(t) is
the
projection of the rotation angular velocity of the earth in the body
coordinate system, vh(t)
is the velocity over ground of the shearer, fcsfh(t) is the specific force
measured by an
accelerometer in the body coordinate system, and gb is the gravity
acceleration of the body
coordinate system;
then deriving a multi-vector attitude determination equation from the specific
force
equation of the strap-down inertial navigation in combination with the
velocity over ground
rb" o" (t) ; and
of the shearer obtained in step D: v(t) = c
F. selecting m different integration moments and constructing m non-coplanar
vectors in
a three-dimensional space:
v
r cr
(i =1,2, ...m)
--= b v
and finally, solving an initial attitude matrix of the strap-down inertial
navigation by
using a Whaba optimal matrix, so as to achieve the initial alignment of the
strap-down inertial
navigation system.
Compared with the prior art, the present invention utilizes a camera mounted
on a
hydraulic support, obtains the motion direction and the actual velocity over
ground of the
shearer in combination with the optical flow technology, derives a multi-
vector attitude
determination equation from a specific force equation of strap-down inertial
navigation, and
finally, solves an initial attitude matrix of the strap-down inertial
navigation by using a
Whaba optimal matrix, so as to achieve the initial alignment of the strap-down
inertial
navigation system. The present invention uses external velocity assistance
without the need
for a coarse alignment stage to achieve accurate initial alignment of a
movable base of the
strap-down inertial navigation. In addition, the combination of optical flow
technology and
strap-down inertial navigation technology can further reduce the error of the
attitude angle of
the shearer, improving the error correction effect for the accurate initial
alignment of the
movable base of the shearer.
4
BRIEF DESCRIPTION OF THE DRAWINGS
FIG. 1 is a schematic structural diagram according to the present invention;
FIG. 2 is a schematic diagram of a two-dimensional projection of a three-
dimensional
object moving at a point according to the present invention;
FIG. 3 is a flowchart of detecting the velocity of a shearer in combination
with the
optical flow method according to the present invention; and
FIG. 4 is a flowchart of initial alignment of inertial navigation according to
the present
invention.
In the drawings, 1: shearer; 2: explosion-proof box; 3: strap-down inertial
navigation
system; 4: processor; 5: hydraulic support; 6: fixed support; 7: camera.
DETAILED DESCRIPTION OF THE INVENTION
The present invention is further described below.
As shown in the drawings, an initial alignment system for strap-down inertial
navigation
of a shearer based on an optical flow method comprises an explosion-proof box
2, a
strap-down inertial navigation system 3, a processor 4, a fixed support 6, and
a camera 7,
wherein the explosion-proof box 2 is fixedly mounted on the body of the
shearer 1; the
strap-down inertial navigation system 6 and the processor 4 are mounted in the
explosion-proof box 2; the camera 7 is fixed on a hydraulic support 5 at one
side of the
shearer 1 by means of the fixed support 6, with the photographing direction of
the camera 7
facing toward the shearer 1.
Further, the processor 4 comprises a micro-processing unit module, a
communication
module, an alarm module, a data storage module, an isolation circuit, and a
power module.
The micro-processing unit module is connected to the communication module, the
alarm
module, the data storage module, the isolation circuit, and the power module,
respectively.
Further, the micro-processing unit module of the processor 4 selects a DSP
chip. The
DSP chip is used to collect and process the data collected by the strap-down
inertial
navigation system and the camera and may be one available from Texas
Instruments
Incorporated, such as its TMS320F28035 Real-Time Microcontroller.
Further, the explosion-proof box 2 is a special explosion-proof box for coal
mines.
Date Recue/Date Received 2021-08-05
CA 03071299 2020-01-28
Further, the camera 7 is hingedly connected to the fixed support 6. This
connection
mode allows the camera 7 to rotate 360 degrees around the fixed support 6.
Further, the strap-down inertial navigation system 3 adopts a laser strap-down
inertial
navigation system, wherein the random drift stability of a laser gyroscope is
0.01 /h, and the
bias stability of an accelerometer is 10-5 g.
An initial alignment method for strap-down inertial navigation of shearer
based on
optical flow method comprises the following specific steps.
A. A camera 7 photographs an image of the environment where a shearer I is
located
with the camera frame rate of 25 frames/s, and transmits the photographed
image to a
processor 4.
B. The processor 4 performs gray-scale processing on the photographed image in
an
image gray-scale mode. When the shearer 1 moves in the photographing
environment, the
photographed target image changes, and the surface motion of the image gray-
scale mode is
optical flow, and the optical flow at each point in the image forms an optical
flow field. The
optical flow field is a two-dimensional instantaneous velocity field in which
a
two-dimensional velocity field vector is the projection of a three-dimensional
velocity vector
of a visible point in the scene on an imaging surface. If a velocity vector is
assigned to each
pixel point in the image, an image motion field is formed. At a particular
moment in the
motion, a certain point pi in the image corresponds to a certain point P0 on
the shearer.
This correspondence can be obtained from the projection equation. In the case
of perspective
projection, a line connecting a point in the image to a corresponding point on
the object
passes through the optical center, and is called an image point connecting
line (point ray), as
shown in FIG. 2.
The relational model is as follows: assuming that a point pa on the object has
a
velocity vo relative to the camera, a corresponding projection point p, on the
image plane
has a velocity vz . The point pa moves by vzift at a time interval at . The
velocity is
expressed by the following formula:
1
-
v_ Cirg
laL dt
dr I
V f II.
dt (1)
6
CA 03071299 2020-01-28
wherein the motion relationship between ro and r: is:
vt . Tit
I r (2)
wherein f is the focal length of a lens, and z is the distance from the center
of the lens to
the target. The velocity vector relationship, as shown in formula (3), given
to each pixel is
obtained by formula (2) and formula (1), and these vectors form the motion
field.
Vi dri f (3)
The relationship between the motion velocity of the three-dimensional object
and the
projection velocity of the image plane can be obtained from formula (3).
The moving direction of the shearer 1 is determined based on the principle of
principal
direction of motion according to the relationship between a motion field and
an optical flow
field of the shearer 1.
C. The optical flow velocities in the horizontal and vertical directions of
each point on
the optical flow image are calculated by using the Lucas-Kanade optical flow
method, and
average values u and v of the optical flow velocities in the horizontal and
vertical directions
of these feature points are calculated. The calculation formula is as follows:
1
M ft
U 711UKXrY)
I
I
V M Vi (Xr 17)
II
I =VI
Then, the macroscopic optical velocity of the moving object can be
obtained,
and the calculation formula is as follows:
I = 2 + v2
According to formula (3), the velocity in pixels can be converted into the
velocity in
distances, and the actual moving velocity of the shearer can be obtained:
mi kttil
7
CA 03071299 2020-01-28
Thus, the velocity information in the motion direction of the shearer 1 is
obtained.
D. Specific force information is projected onto an inertial coordinate system
by using a
specific force equation of strap-down inertial navigation, to obtain direction
change
information the specific force relative to the inertial space as the earth
rotates, with the
specific force equation being:
= gb
wherein a6(t) is the angular velocity of a body coordinate system, oi,;(1) is
the
projection of the rotation angular velocity of the earth in the body
coordinate system, vb(t)
is= the velocity over ground of the shearer, fib (t) is the specific force
measured by an
accelerometer in the body coordinate system, and gb is the gravity
acceleration of the body
coordinate system.
Both sides are then simultaneously multiplied by a CI; matrix in combination
with the
velocity over ground of the shearer 1 obtained in step D, to obtain the
following formula after
finishing:
C,': {C'bh (Wb (t)+ C (0(0),bb (0 + (01,; (0)x vh (0 - C (t) fib =
(t)
wherein given that
u'o (t)= (t)ds
to
(t) = C b'h (t)+ C bib
(t)(4,(t)+ a:,(0)xv"(1)_cbih(offb (t)ds
the multi-vector attitude determination equation is obtained as follows:
v' (t) = C,"6 /41" (t)
E. m different integration moments are selected, and m non-coplanar vectors
are
constructed in a three-dimensional space according to the multi-vector
attitude determination
equation:
8
CA 03071299 2020-01-28
v
,,r. = r
(i = 1,2,...m)
i
Multi-vector attitude determination is to solve the optimal attitude matrix
Chi' that
satisfies the above formula. In order to quantitatively describe the "optimal"
performance (the
meaning of the so-called "optimal" is to minimize the weighted sum square of
the
measurement errors), an index function is constructed:
J(Chr)=-_E w, _ cbrvib 12 =min
2 ,
wherein w, is the known weighting coefficient, Ew, =1, and for an equally-
weighted
average, w, = , and 1,,r ¨Chrv,b reflects an inconsistency error of the same
physical vector
measured in a geographic coordinate system and a carrier coordinate system.
Finally, a
constant matrix 010 is found by using the Whaba optimal matrix solving
algorithm, to
achieve the initial alignment of the strap-down inertial navigation of the
shearer I.
9
