CN111207742B - Coal mining machine positioning and attitude determining method with additional external orientation element constraint - Google Patents

Coal mining machine positioning and attitude determining method with additional external orientation element constraint Download PDF

Info

Publication number
CN111207742B
CN111207742B CN202010053752.XA CN202010053752A CN111207742B CN 111207742 B CN111207742 B CN 111207742B CN 202010053752 A CN202010053752 A CN 202010053752A CN 111207742 B CN111207742 B CN 111207742B
Authority
CN
China
Prior art keywords
coordinate system
error
matrix
positioning
carrier
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.)
Expired - Fee Related
Application number
CN202010053752.XA
Other languages
Chinese (zh)
Other versions
CN111207742A (en
Inventor
龚云
南守琎
石稳欣
王浩森
杨梦佳
高涵
赵吉庆
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Xian University of Science and Technology
Original Assignee
Xian University of Science and Technology
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Xian University of Science and Technology filed Critical Xian University of Science and Technology
Priority to CN202010053752.XA priority Critical patent/CN111207742B/en
Publication of CN111207742A publication Critical patent/CN111207742A/en
Application granted granted Critical
Publication of CN111207742B publication Critical patent/CN111207742B/en
Expired - Fee Related legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/04—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by terrestrial means

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Navigation (AREA)

Abstract

The invention relates to the field of coal cutter control, in particular to a coal cutter positioning and attitude determining method with additional external orientation element constraint, which comprises the following steps: firstly, providing real-time parameters by an inertial navigation system; step two, a vision positioning and attitude determination method; step three, fusing an algorithm; the method has the advantages of strong real-time performance, high sampling frequency, rich data, high attitude determination and positioning precision, acquisition, settlement and feedback of the state parameters of the coal mining machine by using multiple sensors, convenience for controlling the working state of the underground coal mining machine, large amount of redundant data, strong fault tolerance of the system and convenience for controlling the state of the coal mining machine.

Description

Coal mining machine positioning and attitude determining method with additional external orientation element constraint
Technical Field
The invention relates to the field of coal cutter control, in particular to a coal cutter positioning and attitude determining method with additional constraint of external orientation elements.
Background
The traditional coal mining machine orientation attitude determination method depends on known external parameters such as base station coordinates or a traveling track form and cannot meet the requirements of real-time performance, autonomy and accuracy of coal mining machine positioning. The advantages are that: and error accumulation is avoided, and the attitude calculation process is relatively easy. Disadvantages are that: the attitude cannot be determined in real time, the external influence is large in interference, high-precision attitude parameters are difficult to achieve, the time influence is high in influence, the attitude parameters at a certain moment are difficult to determine, and the external control on the operation process of the attitude determination device is extremely difficult.
Self-sensing positioning is carried out by utilizing a gyroscope, an accelerometer and the like, so that the problems of error accumulation, incapability of autonomously correcting deviation and the like exist, and the advantages are as follows: the method has the advantages of real-time performance, high sampling frequency, higher precision and strong anti-interference capability, and is suitable for running in severe environment. Disadvantages are that: because the gyroscope is influenced by zero-bias instability in the long-time output process, errors are gradually accumulated in the attitude calculation process; the comprehensive use of sensor data can cause the problem of insufficient coupling degree; the characteristic of low texture of the underground space environment, poor self-adaptive capacity and inconvenience for controlling the movement of the coal mining machine.
Disclosure of Invention
The invention mainly aims at the disadvantages in the prior art, integrates vision and inertial navigation, provides environmental information by shooting pictures through a camera on one hand, a three-dimensional model of an environmental scene is constructed in the motion process, the position posture of a camera is calculated, pose error compensation information is provided for an inertial navigation system, the inaccuracy problem caused by the drift of inertial error along with time is corrected, on the other hand, the IMU inertial navigation system overcomes the defects of real-time performance and stability of a visual navigation system by virtue of the advantages of high data update rate, high positioning accuracy and no influence of environments such as illumination, temperature and the like, in order to solve the problem that when the angular velocity and the specific force measured by the inertial sensor are integrated for a plurality of times, errors can be accumulated along with time, and because the camera acquires photos frequently and the illumination condition is insufficient, the deviation of position information is generated, and the method for positioning and positioning the coal mining machine with the additional constraint of the external orientation element is provided.
The technical scheme adopted by the invention for solving the technical problems is as follows: a coal mining machine positioning and attitude determining method with additional exterior orientation element constraint is characterized by comprising the following steps:
firstly, providing real-time parameters by an inertial navigation system;
step two, a vision positioning and attitude determination method;
and step three, fusing the algorithm.
The inertial navigation system provides real-time parameters and takes a northeast coordinate system as a navigation coordinate system in IMU positioning and attitude determination, and the specific calculation steps are as follows:
2.1 calculating a transformation matrix from the carrier coordinate system to the navigation coordinate system: the angular change rate measured by the gyroscope is updated through the quaternion, and if the pitch angle is larger than the preset value
Figure GDA0002660373630000021
And the roll angle kappa is very small and is ignored, and a conversion matrix from the carrier coordinate system to the navigation coordinate system is obtained:
Figure GDA0002660373630000022
wherein omega is a course angle
2.2 calculate the position and speed of the carrier: velocity increment corresponding to stress in carrier coordinate system in sampling time delta t measured by accelerometer
Figure GDA0002660373630000023
Conversion to increments in a navigation coordinate system by a transformation matrix
Figure GDA0002660373630000024
Namely, it is
Figure GDA0002660373630000025
The acceleration increment is then:
Figure GDA0002660373630000026
wherein,
Figure GDA0002660373630000031
is an antisymmetric matrix of the rotational angular velocity of the earth,
Figure GDA0002660373630000032
is an antisymmetric matrix of position angular velocity, gLIs the gravity vector, VLIs the speed of the shearer;
after the integration is carried out, the image is obtained,
Figure GDA0002660373630000033
and
Figure GDA0002660373630000034
representing the velocity of the shearer at time k and time k +1,
Figure GDA0002660373630000035
and
Figure GDA0002660373630000036
representing the speed variation from K-1 to K and K +1 of the coal mining machine, the carrierThe speed and position are respectively:
Figure GDA0002660373630000037
2.3 calculate the error of the carrier speed increment: and (3) carrying out derivation on the speed increment to obtain the error of the carrier speed increment:
Figure GDA0002660373630000038
wherein Ω is an antisymmetric matrix of misalignment angle error between the real conversion matrix and the calculated conversion matrix, and if the error V at the previous moment is ignoredLAnd gLThe error of the velocity increment is related to two factors, the current time transformation matrix error and the accelerometer error, i.e. the error of the accelerometer
Figure GDA0002660373630000039
Due to the action of the gravity of the earth,
Figure GDA00026603736300000310
the horizontal position component caused by the large-scale speed increment error is B, and the speed increment error of L is also large;
2.4 calculate position incremental error:
Figure GDA00026603736300000311
where is the integrated error of the position increment.
The visual positioning and attitude determination method comprises the following specific steps:
3.1 solving the coordinates of the target points by using the image stereopair
In the mobile measurement system, a binocular camera is fixedly connected to a carrier, the internal reference of the camera is known, on the basis, equipment installation errors are not considered, the binocular camera stereopair is directly utilized to solve the coordinates of a target point, the position and posture change of the target point in the mobile measurement system is calculated, and the external orientation element is solved:
XT＝λRMXM+X0 X′T＝λ′R′MX′M+X′0 XM＝RPXP
X′M＝R′PX′P
in the formula, XT＝(xT,yT,zT)TIs the coordinate, X, of the ground point A in a rectangular coordinate system of the earth space0＝(x0,y0,z0)TAnd X'0＝(x′0,y′0,z′0)TRespectively the translation amount, X, of the origin of the coordinate system in the geodetic coordinate systemM＝(xm,ym,zm)TAnd X'M＝(x′m,y′m,z′m)TThe translation amount of the coordinates of the ground point A in the left and right image space auxiliary coordinate systems is determined; xP＝(xp,yp,-f)TAnd X'P＝(x′p,y′p,-f′)TThe translation amount of the coordinates of the projection points of the ground point A on the left and right photo under the respective image space coordinate systems; rPAnd R'PThe transformation matrix from a space coordinate system to a photogrammetric coordinate system is accurately given through calibration in the early stage; rMAnd R'MIs a rotation matrix of a photogrammetric coordinate system relative to a geodetic coordinate system and is composed of camera attitude parameters, a course angle omega and a pitch angle
Figure GDA0002660373630000041
And the roll angle kappa, each element in the rotation matrix is a function of the three parameters;
in the measurement operation, when three symmetry axes of two image space auxiliary coordinate systems are parallel to each other, RM＝R′M(ii) a λ, λ' are scale factors representing the ratio of the lengths of the image space coordinate system and the image space auxiliary coordinate system, from the basisThe line length and the coordinates of the ground object point A in the left and right image space auxiliary coordinate systems are calculated, and the formula is as follows:
Figure GDA0002660373630000042
3.2 calculating the binocular camera positioning and attitude determination measurement at each moment:
at the time of k, based on the same name point Ai(i ═ 0 · · n) observation equation:
Figure GDA0002660373630000051
λ(i,k-1)representing the scale factor of the ith camera at time k-1,
Figure GDA0002660373630000052
a rotation matrix representing the photogrammetry coordinate system of the ith camera at time k-1 relative to the geodetic coordinate system.
Coefficient matrix A(i,k)The elements of (a) are described as:
an,m＝f(λ(i,k),αk-1,ωk-1,κk-1,xk-1,yk-1,zk-1,x(i,p,k),y(i,p,k),-f)
f is the main distance of the camera, e(i,L)In order to observe the error vector, the error vector is,
Figure GDA0002660373630000053
representing the attitude and position increment of the left binocular camera between k-1 and k, and adopting a least square method to carry out coefficient matrix A when the number of homonymous points is not less than two(i,k)Solving, and on the basis, realizing the positioning and attitude determination measurement of the binocular camera at each moment through continuous recursion;
the fusion algorithm comprises the following specific steps: in a mobile measurement system, the carrier positioning and attitude determination parameters are subjected to robust estimation by utilizing observation information, and an IMU (inertial measurement Unit) and a binocular camera form self-adaptive fusion filtering based on the robust estimation:
Figure GDA0002660373630000054
wherein L isikCalculating a reference formula:
Figure GDA0002660373630000055
the value of i is 1 and 2, which respectively represent the IMU and the binocular camera sensor,
Figure GDA0002660373630000056
is the robust equivalence weight of each sensor;
based on the self-adaptive Kalman filtering principle, a self-adaptive fusion filtering solution based on robust estimation of each sensor is constructed as follows:
Figure GDA0002660373630000057
Figure GDA0002660373630000058
in the formula:
Figure GDA0002660373630000061
Figure GDA0002660373630000062
Figure GDA0002660373630000063
in the above formula, αkState estimation for adaptive factors using multi-sensor fusion solution
Figure GDA0002660373630000064
The invention has the beneficial effects that: the method has the advantages of strong real-time performance, high sampling frequency, rich data, high attitude determination and positioning precision, acquisition, settlement and feedback of the state parameters of the coal mining machine by using multiple sensors, convenience for controlling the working state of the underground coal mining machine, a large amount of redundant data, strong fault tolerance of the system and convenience for controlling the state of the coal mining machine.
Drawings
FIG. 1 is a schematic flow chart of a coal mining machine positioning and attitude determination method with additional external orientation element constraint according to the invention;
FIG. 2 is a schematic view of a shearer camera installation with additional exterior orientation element constraints in accordance with the present invention;
FIG. 3 is a schematic diagram of a positioning and attitude determination related coordinate system of a coal mining machine camera with additional constraint of exterior orientation elements according to the present invention;
Detailed Description
To further explain the technical means and effects of the present invention adopted to achieve the predetermined object, the following detailed description of the embodiments, structures, features and effects according to the present invention will be given with reference to the accompanying drawings and preferred embodiments.
Example one
As shown in fig. 1, a method for positioning and attitude determination of a coal mining machine with additional constraint of external orientation elements is characterized by comprising the following steps:
firstly, providing real-time parameters by an inertial navigation system;
step two, a vision positioning and attitude determination method;
step three, fusing an algorithm;
the inertial navigation system provides real-time parameters and takes a northeast coordinate system as a navigation coordinate system in IMU positioning and attitude determination, and the specific calculation steps are as follows:
2.1 calculating a transformation matrix from the carrier coordinate system to the navigation coordinate system:
and (3) obtaining a conversion matrix from the carrier coordinate system to the navigation coordinate system by updating the quaternion through the angle change rate measured by the gyroscope:
Figure GDA0002660373630000071
if pitch angle
Figure GDA0002660373630000072
And the roll angle k is very small, neglecting, then
Figure GDA0002660373630000073
Wherein omega is a course angle,
Figure GDA0002660373630000074
Pitch angle and kappa roll angle
2.2 calculate the position and speed of the carrier: velocity increment corresponding to stress in carrier coordinate system in sampling time delta t measured by accelerometer
Figure GDA0002660373630000075
Conversion to increments in a navigation coordinate system by a transformation matrix
Figure GDA0002660373630000076
Namely, it is
Figure GDA0002660373630000077
The acceleration increment is then:
Figure GDA0002660373630000078
wherein,
Figure GDA0002660373630000079
is an antisymmetric matrix of the rotational angular velocity of the earth,
Figure GDA00026603736300000710
is an antisymmetric matrix of position angular velocity, gLIs the gravity vector, VLIs the speed of the shearer;
after the integration is carried out, the image is obtained,
Figure GDA00026603736300000711
and
Figure GDA00026603736300000712
representing the velocity of the shearer at time k and time k +1,
Figure GDA00026603736300000713
and
Figure GDA0002660373630000081
representing the speed variation from the K-1 moment to the K moment of the coal mining machine and from the K moment to the K +1 moment, the speed and the position of the carrier are respectively as follows:
Figure GDA0002660373630000082
Figure GDA0002660373630000083
2.3 calculate the error of the carrier speed increment: and (3) carrying out derivation on the speed increment to obtain the error of the carrier speed increment:
Figure GDA0002660373630000084
wherein,
Figure GDA0002660373630000085
in order to convert the error matrix of the matrix,
Figure GDA0002660373630000086
for the error matrix of the velocity in the derivation process, Ω is an antisymmetric matrix of misalignment angle errors between the true conversion matrix and the calculated conversion matrix, and if the error V at the previous moment is ignoredLAnd gLInfluence of (2), speedThe error of the degree increment is related to two factors, namely the error of the conversion matrix at the current moment and the error of the accelerometer, and the error of the misalignment angle is set as (x y z)THaving an antisymmetric array of
Figure GDA0002660373630000087
Namely, it is
Figure GDA0002660373630000088
Due to the action of the gravity of the earth,
Figure GDA0002660373630000089
the horizontal position component caused by the large-scale speed increment error is B, and the speed increment error of L is also large;
2.4 calculate position incremental error:
Figure GDA00026603736300000810
where is the integrated error of the position increment.
The visual positioning and attitude determination method comprises the following specific steps: as shown in FIG. 3, a mathematical expression of the image points and their corresponding ground points is determined, D-XYZ is the ground photogrammetry coordinate system, S1-U1V1W1And S2-U2V2W2The three axes of the two image space auxiliary coordinate systems are respectively parallel to the three D-XYZ axes. Point A corresponding image point Xp, X'PHas an image space coordinate of (x)p,yp,-f)，(x′p,y′p-f'), B being the photographic baseline, the three components B thereofx、By、Bz。
3.1 solving the coordinates of the target points by using the image stereopair
As shown in fig. 2, 1-6 represent that a plurality of image control points are respectively arranged at equal intervals on two sides of the tunneling of a coal mining machine, and the control points are arranged by adopting a luminescent fluorescent material to enhance the environmental conditions in consideration of the underground coal mining safety and the camera operation environment.
In the mobile measurement system, a binocular camera is fixedly connected to a carrier, the internal reference of the camera is known, on the basis, equipment installation errors are not considered, the binocular camera stereopair is directly utilized to solve the coordinates of a target point, the position and posture change of the target point in the mobile measurement system is calculated, and the external orientation element is solved:
XT＝λRMXM+X0
X′T＝λ′R′MX′M+X′0
XM＝RPXP
X′M＝R′PX′P
in the formula, XT＝(xT,yT,zT)TIs the coordinate, X, of the ground point A in a rectangular coordinate system of the earth space0＝(x0,y0,z0)TAnd X'0＝(x′0,y′0,z′0)TRespectively the translation amount, X, of the origin of the coordinate system in the geodetic coordinate systemM＝(xm,ym,zm)TAnd X'M＝(x′m,y′m,z′m)TThe translation amount of the coordinates of the ground point A in the left and right image space auxiliary coordinate systems is determined; xP＝(xp,yp,-f)TAnd X'P＝(x′p,y′p,-f′)TThe translation amount of the coordinates of the projection points of the ground point A on the left and right photo under the respective image space coordinate systems; rPAnd R'PThe transformation matrix from a space coordinate system to a photogrammetric coordinate system is accurately given through calibration in the early stage; rMAnd R'MIs a rotation matrix of a photogrammetric coordinate system relative to a geodetic coordinate system and is composed of camera attitude parameters, a course angle omega and a pitch angle
Figure GDA0002660373630000101
And the roll angle kappa, each element in the rotation matrix is a function of the three parameters;
in the measurement operation, when three symmetry axes of two image space auxiliary coordinate systems are parallel to each other, RM＝R′M(ii) a λ and λ' are scale factors, which represent the length ratio of the image space coordinate system and the image space auxiliary coordinate system, and are calculated from the base length and the coordinates of the ground object point a in the left and right image space auxiliary coordinate systems, and the formula is as follows:
Figure GDA0002660373630000102
3.2 calculating the binocular camera positioning and attitude determination measurement at each moment:
at the time of k, based on the same name point AiThe observation equation of (a):
Figure GDA0002660373630000103
wherein i is 0. n, λ(i,k-1)Representing the scale factor of the ith camera at time k-1,
Figure GDA0002660373630000104
a rotation matrix representing the photogrammetry coordinate system of the ith camera at time k-1 relative to the geodetic coordinate system;
coefficient matrix A(i,k)The elements of (a) are described as:
an,m＝f(λ(i,k),αk-1,ωk-1,κk-1,xk-1,yk-1,zk-1,x(i,p,k),y(i,p,k),-f)
f is the main distance of the camera, e(i,L)In order to observe the error vector, the error vector is,
Figure GDA0002660373630000105
indicating left binocular camera at k-1 to kThe attitude and position increment between scales is a parameter to be estimated, and when the number of the same-name points is not less than two, the least square method is adopted to carry out the coefficient matrix A(i,k)Solving, and on the basis, realizing the positioning and attitude determination measurement of the binocular camera at each moment through continuous recursion;
the fusion algorithm comprises the following specific steps: in a mobile measurement system, the carrier positioning and attitude determination parameters are subjected to robust estimation by utilizing observation information, and an IMU (inertial measurement Unit) and a binocular camera form self-adaptive fusion filtering based on the robust estimation:
Figure GDA0002660373630000111
wherein L isikCalculating a reference formula:
Figure GDA0002660373630000112
the value of i is 1 and 2, which respectively represent the IMU and the binocular camera sensor,
Figure GDA0002660373630000113
is the robust equivalence weight of each sensor;
based on the self-adaptive Kalman filtering principle, a self-adaptive fusion filtering solution based on robust estimation of each sensor is constructed as follows:
Figure GDA0002660373630000114
Figure GDA0002660373630000115
in the formula:
Figure GDA0002660373630000116
Figure GDA0002660373630000117
Figure GDA0002660373630000118
in the above formula, αkState estimation for adaptive factors using multi-sensor fusion solution
Figure GDA0002660373630000119
According to the structural formula, when the self-adaptive factor is carried out on the mismatch value of the basic state, no matter which method is adopted, the self-adaptive factor can be automatically adjusted according to the conformity of the dynamic model and the motion state of the carrier, and when the conformity of the dynamic model and the motion state of the carrier is better, alpha iskApproaching 1, as in the normal fusion filtering solution. Since the estimation is made on the basis of the robust, when the two are in poor conformity, the predicted value has a large error in statistical probability, and in this case, αkBecomes smaller and even tends to zero, the influence of the error of the prediction vector is reduced, and the accuracy of the filtering result is more determined by the observed value.
Furthermore, the invention relates to a triaxial optical fiber gyroscope for measuring the angular speed variation of a coal mining machine, namely omega is a course angle,
Figure GDA0002660373630000121
The pitch angle and kappa are three components of the roll angle; the three-axis accelerometer provides three acceleration components of the coal mining machine, the optical fiber gyroscope and the accelerometer are combined into an inertial navigation system, and attitude parameters of the coal mining machine under the inertial navigation system are calculated through acceleration integral and attitude angle rotation conditions; and the camera is used for shooting a picture to provide environment information, constructing a three-dimensional model of an environment scene in a motion process, and calculating the posture of the camera through the scene in reverse. Because the camera is fixedly connected with the coal mining machine, the attitude of the camera can reflect the attitude of the coal mining machine in the motion process, so that the attitude parameters obtained by calculation of the gyroscope and the accelerometer are mutually fused, and the attitude of the coal mining machine in the motion advancing process is finally determined.
Further, the coordinate system related to the coal mining machine positioning and attitude determination method with the additional constraint of the external orientation element mainly comprises the following steps:
navigation coordinate system: solving a reference coordinate system used by the navigation parameter by the inertial navigation system;
a carrier coordinate system: the center of mass of the carrier is used as an origin, OX points to the right wing along the direction of a longitudinal axis, namely the advancing direction of the carrier, the Z axis points to the right wing along the direction of a lateral axis of the carrier, and Y points to the right wing along the direction of a vertical axis of the carrier, namely a right-hand coordinate system (namely pointing to the sky). In general, the relation between the carrier coordinate system and the geographic coordinate system is the attitude of the carrier;
image space coordinate system: in order to perform spatial coordinate conversion of the image point, a coordinate system describing the image point at the image space position needs to be established;
image space auxiliary coordinate system: the image space coordinate of the image point is directly obtained from the plane coordinate of the image, but the image space coordinate system of each image is not uniform, which brings difficulty to calculation, therefore, a relatively uniform coordinate system needs to be established and expressed by s-uvw;
ground measurement coordinate system: generally, the left-handed rectangular coordinate system is a space left-handed rectangular coordinate system formed by combining a plane rectangular coordinate projected by a Gauss-Kluger 6-degree band or a 3-degree band (or any band) under a space geodetic coordinate reference and a defined elevation measured from a certain reference plane and using T-XtYtZtRepresents;
geodetic coordinate system: the coordinate system is established by taking a reference ellipsoid as a datum plane in geodetic measurement;
terrestrial photogrammetry coordinate system: the ground point is converted from the auxiliary coordinate system of image space to a transitional coordinate system established between the coordinate systems of ground measurement.
On one hand, the visual inertial navigation combination provides environmental information by shooting pictures through a camera, constructs a three-dimensional model of an environmental scene in the motion process, calculates the position and posture of the camera, provides pose error compensation information for an inertial navigation system and corrects the inaccuracy problem of an IMU (inertial measurement Unit) caused by the drift of inertial errors along with time; on the other hand, the IMU inertial navigation system overcomes the defects of real-time performance and stability of the visual navigation system by virtue of the advantages of high data update rate, high positioning accuracy and no influence of environments such as illumination, temperature and the like. The advantages of the two are combined, and the precision and the robustness of the positioning system are improved. However, the angular velocity and the specific force measured by the inertial sensor are integrated for several times to respectively obtain navigation information such as attitude, velocity and position, errors can be accumulated along with time, and in order to solve the problem, external information feedback must be introduced by a non-inertial sensor, and the non-inertial sensor and the inertial system form a stable system together to control the divergence of the errors of the inertial navigation system. The pose calculation of the vision system can generate the deviation of position information due to the low frequency of the camera for collecting photos, the insufficient illumination condition and the like. The system has deviation when measuring position information, and in order to solve the problem, a coal mining machine positioning and attitude determining method with additional external orientation element constraint is provided. The method has the advantages of strong real-time performance, high sampling frequency, rich data, high attitude determination and positioning precision, acquisition, settlement and feedback of the state parameters of the coal mining machine by using multiple sensors, convenience for controlling the working state of the underground coal mining machine, a large amount of redundant data, strong fault tolerance of the system and convenience for controlling the state of the coal mining machine.

Claims (3)

1. A coal mining machine positioning and attitude determining method with additional exterior orientation element constraint is characterized by comprising the following steps:
firstly, providing real-time parameters by an inertial navigation system;
step two, the vision positioning and attitude determination method specifically comprises the following steps:
1.1 solving the coordinates of the target point by using the image stereopair
In the mobile measurement system, a binocular camera is fixedly connected to a carrier, the internal reference of the camera is known, on the basis, equipment installation errors are not considered, the binocular camera stereopair is directly utilized to solve the coordinates of a target point, the position and posture change of the target point in the mobile measurement system is calculated, and the external orientation element is solved:
XT＝λRMXM+X0 X′T＝λ′R′MX′M+X′0 XM＝RPXP
X′M＝R′pX′p
in the formula, XT＝(xT,yT,zT)TIs the coordinate, X, of the ground point A in a rectangular coordinate system of the earth space0＝(x0,y0,z0)TAnd X'0＝(x′0,y′0,z′0)TRespectively the translation amount, X, of the origin of the coordinate system in the geodetic coordinate systemM＝(xm,ym,zm)TAnd X'M＝(x′m,y′m,z′m)TThe translation amount of the coordinates of the ground point A in the left and right image space auxiliary coordinate systems is determined; xP＝(xp,yp,-f)TAnd X'p＝(x′p,y′p,-f′)TThe translation amount of the coordinates of the projection points of the ground point A on the left and right photo under the respective image space coordinate systems; f and f' represent focal lengths of the left and right cameras, respectively; rPAnd R'pThe transformation matrix from a space coordinate system to a photogrammetric coordinate system is accurately given through calibration in the early stage; rMAnd R'MIs a rotation matrix of a photogrammetric coordinate system relative to a geodetic coordinate system and is composed of camera attitude parameters, a course angle omega and a pitch angle
Figure FDA0002750482290000011
And the roll angle kappa, each element in the rotation matrix is a function of the three parameters;
in the measurement operation, when three symmetry axes of two image space auxiliary coordinate systems are parallel to each other, RM＝R′M(ii) a λ and λ' are scale factors, which represent the length ratio of the image space coordinate system and the image space auxiliary coordinate system, and are calculated from the base length and the coordinates of the ground object point a in the left and right image space auxiliary coordinate systems, and the formula is as follows:
Figure FDA0002750482290000012
1.2 calculating the binocular camera positioning and attitude determination measurement at each moment:
at the time of k, based on the same name point AiThe observation equation of (a):
Figure FDA0002750482290000021
wherein i is 0 … n, λ(i,k-1)Representing the scale factor of the ith camera at time k-1,
Figure FDA0002750482290000022
a rotation matrix representing the photogrammetry coordinate system of the ith camera at time k-1 relative to the geodetic coordinate system;
the elements of the coefficient matrix a (i, k) are described as:
an,m＝f(λ(i,k),αk-1,ωk-1,κk-1,xκk-1,yk-1,zk-1,x(i,p,k),y(i,p,k),-f)
f is the main distance of the camera, e(i,L)In order to observe the error vector, the error vector is,
Figure FDA0002750482290000023
representing the posture and position increment of the left binocular camera between k-1 and k, wherein the posture and position increment are parameters to be estimated, when the number of the same-name points is not less than two, solving a coefficient matrix A (i, k) by adopting a least square method, and realizing positioning and posture-fixing measurement of the binocular camera at each time through continuous recursion on the basis;
step three, fusion algorithm: the IMU and the binocular camera form a self-adaptive fusion algorithm based on robust estimation.
2. The method for positioning and attitude determination of the coal mining machine with the additional constraint of the outer orientation elements according to claim 1, is characterized in that: the inertial navigation system provides real-time parameters and takes a northeast coordinate system as a navigation coordinate system in IMU positioning and attitude determination, and the specific calculation steps are as follows:
2.1 calculating a transformation matrix from the carrier coordinate system to the navigation coordinate system: the angular change rate measured by the gyroscope is updated through the quaternion, and if the pitch angle is larger than the preset value
Figure FDA0002750482290000024
And the roll angle kappa is very small and ignored, and a conversion matrix from the carrier coordinate system to the navigation coordinate system is obtained:
Figure FDA0002750482290000025
wherein, omega is a course angle;
2.2 calculate the position and speed of the carrier: velocity increment corresponding to stress in carrier coordinate system in sampling time delta t measured by accelerometer
Figure FDA0002750482290000026
Conversion to increments in a navigation coordinate system by a transformation matrix
Figure FDA0002750482290000027
Namely, it is
Figure FDA0002750482290000028
The acceleration increment is then:
Figure FDA0002750482290000029
wherein,
Figure FDA00027504822900000210
is an antisymmetric matrix of the rotational angular velocity of the earth,
Figure FDA00027504822900000211
is an antisymmetric matrix of position angular velocity, gLIs the gravity vector, VLIs the speed of the shearer;
after the integration is carried out, the image is obtained,
Figure FDA0002750482290000031
and
Figure FDA0002750482290000032
representing the velocity of the shearer at time K and time K +1,
Figure FDA0002750482290000033
and
Figure FDA0002750482290000034
and representing the speed variation from the K-1 moment to the K moment of the coal mining machine and from the K moment to the K +1 moment, and the speed and the position of the carrier are respectively as follows:
Figure FDA0002750482290000035
2.3 calculate the error of the carrier speed increment: and (3) carrying out derivation on the speed increment to obtain the error of the carrier speed increment:
Figure FDA0002750482290000036
wherein,
Figure FDA0002750482290000037
in order to convert the error matrix of the matrix,
Figure FDA0002750482290000038
for the error matrix of the velocity in the derivation process, Ω is an antisymmetric matrix of misalignment angle errors between the true conversion matrix and the calculated conversion matrix, and if the error V at the previous moment is ignoredLAnd gLThe error of the velocity increment is related to two factors, the current time transformation matrix error and the accelerometer error, i.e. the error of the accelerometer
Figure FDA0002750482290000039
Due to the action of the gravity of the earth,
Figure FDA00027504822900000310
the horizontal position component caused by the large-scale speed increment error is B, and the speed increment error of L is also large;
2.4 calculate position incremental error:
Figure FDA00027504822900000311
where is the integrated error of the position increment.
3. The method for positioning and attitude determination of the coal mining machine with the additional constraint of the outer orientation elements according to claim 1, is characterized in that: the fusion algorithm comprises the following specific steps: in a mobile measurement system, the carrier positioning and attitude determination parameters are subjected to robust estimation by utilizing observation information, and an IMU (inertial measurement Unit) and a binocular camera form self-adaptive fusion filtering based on the robust estimation:
Figure FDA0002750482290000041
wherein,
Figure FDA0002750482290000042
the value of i is 1 and 2, which respectively represent the IMU and the binocular camera sensor,
Figure FDA0002750482290000043
is the robust equivalence weight of each sensor;
based on the self-adaptive Kalman filtering principle, the self-adaptive fusion filtering solution based on the robust estimation of each sensor is constructed as follows:
Figure FDA0002750482290000044
Figure FDA0002750482290000045
in the formula:
Figure FDA0002750482290000046
Figure FDA0002750482290000047
Figure FDA0002750482290000048
in the above formula, αkState estimation for adaptive factors using multi-sensor fusion solution
Figure FDA0002750482290000049
CN202010053752.XA 2020-01-17 2020-01-17 Coal mining machine positioning and attitude determining method with additional external orientation element constraint Expired - Fee Related CN111207742B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010053752.XA CN111207742B (en) 2020-01-17 2020-01-17 Coal mining machine positioning and attitude determining method with additional external orientation element constraint

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010053752.XA CN111207742B (en) 2020-01-17 2020-01-17 Coal mining machine positioning and attitude determining method with additional external orientation element constraint

Publications (2)

Publication Number Publication Date
CN111207742A CN111207742A (en) 2020-05-29
CN111207742B true CN111207742B (en) 2020-12-15

Family

ID=70782304

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010053752.XA Expired - Fee Related CN111207742B (en) 2020-01-17 2020-01-17 Coal mining machine positioning and attitude determining method with additional external orientation element constraint

Country Status (1)

Country Link
CN (1) CN111207742B (en)

Families Citing this family (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112180319B (en) * 2020-08-21 2022-11-29 天津市山石机器人有限责任公司 Space positioning system based on left and right chirality and positioning method thereof
CN112857367B (en) * 2021-01-21 2023-10-13 中国煤炭科工集团太原研究院有限公司 Heading machine pose detection method based on machine vision and inertial navigation
CN112902952B (en) * 2021-01-21 2023-11-10 中国煤炭科工集团太原研究院有限公司 Heading machine pose detection method based on millimeter wave radar and inertial navigation

Family Cites Families (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103424114B (en) * 2012-05-22 2016-01-20 同济大学 A kind of full combined method of vision guided navigation/inertial navigation
US9607401B2 (en) * 2013-05-08 2017-03-28 Regents Of The University Of Minnesota Constrained key frame localization and mapping for vision-aided inertial navigation
CN108613675B (en) * 2018-06-12 2021-07-20 武汉大学 Low-cost unmanned aerial vehicle movement measurement method and system
CN109115172B (en) * 2018-08-23 2020-09-08 西安科技大学 Machine vision-based method for detecting pose of body of coal mining machine
CN110411443B (en) * 2019-06-05 2023-05-02 西京学院 Coal mining machine rocker arm inertia/vision combined attitude determination device and method

Also Published As

Publication number Publication date
CN111207742A (en) 2020-05-29

Similar Documents

Publication Publication Date Title
CN111207742B (en) Coal mining machine positioning and attitude determining method with additional external orientation element constraint
CN110243358B (en) Multi-source fusion unmanned vehicle indoor and outdoor positioning method and system
CN109269471B (en) Novel GNSS receiver inclination measuring system and method
CN105698765B (en) Object pose method under double IMU monocular visions measurement in a closed series noninertial systems
CN101788296B (en) SINS/CNS deep integrated navigation system and realization method thereof
CN106289246B (en) A kind of flexible link arm measure method based on position and orientation measurement system
CN103994763B (en) The SINS/CNS deep integrated navigation system of a kind of Marsokhod and its implementation
CN110411444B (en) Inertial navigation positioning system and positioning method for underground mining mobile equipment
CN107270893B (en) Lever arm and time asynchronous error estimation and compensation method for real estate measurement
CN104006787B (en) Spacecraft Attitude motion simulation platform high-precision attitude defining method
CN107607113B (en) Method for measuring inclination angles of two-axis attitude
CN107063246A (en) A kind of Loosely coupled air navigation aid of vision guided navigation/inertial navigation
CN110702109B (en) Coal mining machine inertial navigation/wireless sensor network combined positioning method
CN105547288A (en) Self-localization method and system for mobile device in underground coal mine
KR100860767B1 (en) Apparatus and method for digital mapping manufacture using airborne laser surveying data
CN112378399B (en) Coal mine tunnel tunneling robot precise positioning and orientation method based on strapdown inertial navigation and digital total station
CN104697520B (en) Integrated gyro free strap down inertial navigation system and gps system Combinated navigation method
CN110325822B (en) Cradle head pose correction method and cradle head pose correction device
CN111637888B (en) Tunneling machine positioning method and system based on inertial navigation and laser radar single-point distance measurement
US20170074678A1 (en) Positioning and orientation data analysis system and method thereof
KR100558367B1 (en) System and method for making digital map using gps and ins
CN113465599B (en) Positioning and orientation method, device and system
CN114485643B (en) Coal mine underground mobile robot environment sensing and high-precision positioning method
El-Sheimy Georeferencing component of LiDAR systems
CN111323011A (en) Coal mining machine body and rocker arm cooperative positioning device and positioning method

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant
CF01 Termination of patent right due to non-payment of annual fee
CF01 Termination of patent right due to non-payment of annual fee

Granted publication date: 20201215

Termination date: 20220117